├── .gitattributes ├── .gitignore ├── LICENSE ├── Physics2D ├── include │ ├── physics2d.h │ ├── physics2d_aabb.h │ ├── physics2d_algorithm_2d.h │ ├── physics2d_body.h │ ├── physics2d_capsule.h │ ├── physics2d_ccd.h │ ├── physics2d_circle.h │ ├── physics2d_collider.h │ ├── physics2d_common.h │ ├── physics2d_contact.h │ ├── physics2d_detector.h │ ├── physics2d_distance_joint.h │ ├── physics2d_edge.h │ ├── physics2d_ellipse.h │ ├── physics2d_grid.h │ ├── physics2d_integrator.h │ ├── physics2d_joint.h │ ├── physics2d_joints.h │ ├── physics2d_linear.h │ ├── physics2d_math.h │ ├── physics2d_matrix2x2.h │ ├── physics2d_matrix3x3.h │ ├── physics2d_matrix4x4.h │ ├── physics2d_narrowphase.h │ ├── physics2d_point_joint.h │ ├── physics2d_polygon.h │ ├── physics2d_pulley_joint.h │ ├── physics2d_quaternion.h │ ├── physics2d_random.h │ ├── physics2d_rectangle.h │ ├── physics2d_revolute_joint.h │ ├── physics2d_rotation_joint.h │ ├── physics2d_sap.h │ ├── physics2d_shape.h │ ├── physics2d_simplex.h │ ├── physics2d_system.h │ ├── physics2d_tree.h │ ├── physics2d_vector2.h │ ├── physics2d_vector3.h │ ├── physics2d_vector4.h │ ├── physics2d_weld_joint.h │ └── physics2d_world.h └── source │ ├── collision │ ├── physics2d_aabb.cpp │ ├── physics2d_capsule.cpp │ ├── physics2d_ccd.cpp │ ├── physics2d_circle.cpp │ ├── physics2d_collider.cpp │ ├── physics2d_detector.cpp │ ├── physics2d_edge.cpp │ ├── physics2d_ellipse.cpp │ ├── physics2d_grid.cpp │ ├── physics2d_narrowphase.cpp │ ├── physics2d_polygon.cpp │ ├── physics2d_rectangle.cpp │ ├── physics2d_sap.cpp │ ├── physics2d_simplex.cpp │ └── physics2d_tree.cpp │ ├── dynamics │ ├── physics2d_body.cpp │ ├── physics2d_contact.cpp │ ├── physics2d_system.cpp │ └── physics2d_world.cpp │ ├── math │ ├── physics2d_algorithm_2d.cpp │ ├── physics2d_integrator.cpp │ ├── physics2d_math.cpp │ ├── physics2d_matrix2x2.cpp │ ├── physics2d_matrix3x3.cpp │ ├── physics2d_matrix4x4.cpp │ ├── physics2d_quaternion.cpp │ ├── physics2d_vector2.cpp │ ├── physics2d_vector3.cpp │ └── physics2d_vector4.cpp │ └── other │ ├── physics2d_common.cpp │ └── physics2d_random.cpp ├── README.md ├── README_zh_CN.md └── xmake.lua /.gitattributes: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # Set default behavior to automatically normalize line endings. 3 | ############################################################################### 4 | * text=auto 5 | 6 | ############################################################################### 7 | # Set default behavior for command prompt diff. 8 | # 9 | # This is need for earlier builds of msysgit that does not have it on by 10 | # default for csharp files. 11 | # Note: This is only used by command line 12 | ############################################################################### 13 | #*.cs diff=csharp 14 | 15 | ############################################################################### 16 | # Set the merge driver for project and solution files 17 | # 18 | # Merging from the command prompt will add diff markers to the files if there 19 | # are conflicts (Merging from VS is not affected by the settings below, in VS 20 | # the diff markers are never inserted). Diff markers may cause the following 21 | # file extensions to fail to load in VS. An alternative would be to treat 22 | # these files as binary and thus will always conflict and require user 23 | # intervention with every merge. To do so, just uncomment the entries below 24 | ############################################################################### 25 | #*.sln merge=binary 26 | #*.csproj merge=binary 27 | #*.vbproj merge=binary 28 | #*.vcxproj merge=binary 29 | #*.vcproj merge=binary 30 | #*.dbproj merge=binary 31 | #*.fsproj merge=binary 32 | #*.lsproj merge=binary 33 | #*.wixproj merge=binary 34 | #*.modelproj merge=binary 35 | #*.sqlproj merge=binary 36 | #*.wwaproj merge=binary 37 | 38 | ############################################################################### 39 | # behavior for image files 40 | # 41 | # image files are treated as binary by default. 42 | ############################################################################### 43 | #*.jpg binary 44 | #*.png binary 45 | #*.gif binary 46 | 47 | ############################################################################### 48 | # diff behavior for common document formats 49 | # 50 | # Convert binary document formats to text before diffing them. This feature 51 | # is only available from the command line. Turn it on by uncommenting the 52 | # entries below. 53 | ############################################################################### 54 | #*.doc diff=astextplain 55 | #*.DOC diff=astextplain 56 | #*.docx diff=astextplain 57 | #*.DOCX diff=astextplain 58 | #*.dot diff=astextplain 59 | #*.DOT diff=astextplain 60 | #*.pdf diff=astextplain 61 | #*.PDF diff=astextplain 62 | #*.rtf diff=astextplain 63 | #*.RTF diff=astextplain 64 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | ## Ignore Visual Studio temporary files, build results, and 2 | ## files generated by popular Visual Studio add-ons. 3 | ## 4 | ## Get latest from https://github.com/github/gitignore/blob/master/VisualStudio.gitignore 5 | 6 | # MacOS Cache 7 | .DS_Store 8 | 9 | # Xmake cache 10 | .xmake/ 11 | build/ 12 | makefile 13 | 14 | # gcc cache 15 | gcm.cache 16 | 17 | # for VS Code 18 | .vscode/ 19 | 20 | # for vim 21 | *.swp 22 | *.swo 23 | tags 24 | !tags/ 25 | 26 | # Ignore packaging files 27 | winenv/ 28 | *.exe 29 | *.zip 30 | *.gz 31 | *.bz2 32 | *.xz 33 | *.7z 34 | *.run 35 | 36 | # for compiler 37 | compile_commands.json 38 | 39 | # Makefile generation 40 | /core/xmake.config.h 41 | /core/.config.mak 42 | /core/**/*.o 43 | /core/**/*.b 44 | /core/**/*.a 45 | /core/**/*.obj 46 | /core/**/*.lib 47 | /core/**/*.exe 48 | 49 | # for fortran 50 | /tests/**/*.mod 51 | 52 | # for rpmbuild 53 | /scripts/rpmbuild/BUILD*/* 54 | /scripts/rpmbuild/RPMS/* 55 | /scripts/rpmbuild/SOURCES/* 56 | /scripts/rpmbuild/SRPMS/* 57 | 58 | !/xmake/actions/build/ 59 | 60 | # for linux driver 61 | *.o 62 | *.ko 63 | *.mod 64 | .*.cmd 65 | *.mod.c 66 | Module.symvers 67 | modules.order 68 | 69 | # User-specific files 70 | *.rsuser 71 | *.suo 72 | *.user 73 | *.userosscache 74 | *.sln.docstates 75 | 76 | # User-specific files (MonoDevelop/Xamarin Studio) 77 | *.userprefs 78 | 79 | # Mono auto generated files 80 | mono_crash.* 81 | 82 | # Build results 83 | [Dd]ebug/ 84 | [Dd]ebugPublic/ 85 | [Rr]elease/ 86 | [Rr]eleases/ 87 | x64/ 88 | x86/ 89 | [Ww][Ii][Nn]32/ 90 | [Aa][Rr][Mm]/ 91 | [Aa][Rr][Mm]64/ 92 | bld/ 93 | [Bb]in/ 94 | [Oo]bj/ 95 | [Oo]ut/ 96 | [Ll]og/ 97 | [Ll]ogs/ 98 | 99 | # Visual Studio 2015/2017 cache/options directory 100 | .vs/ 101 | # Uncomment if you have tasks that create the project's static files in wwwroot 102 | #wwwroot/ 103 | 104 | # Visual Studio 2017 auto generated files 105 | Generated\ Files/ 106 | 107 | # MSTest test Results 108 | [Tt]est[Rr]esult*/ 109 | [Bb]uild[Ll]og.* 110 | 111 | # NUnit 112 | *.VisualState.xml 113 | TestResult.xml 114 | nunit-*.xml 115 | 116 | # Build Results of an ATL Project 117 | [Dd]ebugPS/ 118 | [Rr]eleasePS/ 119 | dlldata.c 120 | 121 | # Benchmark Results 122 | BenchmarkDotNet.Artifacts/ 123 | 124 | # .NET Core 125 | project.lock.json 126 | project.fragment.lock.json 127 | artifacts/ 128 | 129 | # ASP.NET Scaffolding 130 | ScaffoldingReadMe.txt 131 | 132 | # StyleCop 133 | StyleCopReport.xml 134 | 135 | # Files built by Visual Studio 136 | *_i.c 137 | *_p.c 138 | *_h.h 139 | *.ilk 140 | *.meta 141 | *.obj 142 | *.iobj 143 | *.pch 144 | *.pdb 145 | *.ipdb 146 | *.pgc 147 | *.pgd 148 | *.rsp 149 | *.sbr 150 | *.tlb 151 | *.tli 152 | *.tlh 153 | *.tmp 154 | *.tmp_proj 155 | *_wpftmp.csproj 156 | *.log 157 | *.vspscc 158 | *.vssscc 159 | .builds 160 | *.pidb 161 | *.svclog 162 | *.scc 163 | 164 | # Chutzpah Test files 165 | _Chutzpah* 166 | 167 | # Visual C++ cache files 168 | ipch/ 169 | *.aps 170 | *.ncb 171 | *.opendb 172 | *.opensdf 173 | *.sdf 174 | *.cachefile 175 | *.VC.db 176 | *.VC.VC.opendb 177 | 178 | # Visual Studio profiler 179 | *.psess 180 | *.vsp 181 | *.vspx 182 | *.sap 183 | 184 | # Visual Studio Trace Files 185 | *.e2e 186 | 187 | # TFS 2012 Local Workspace 188 | $tf/ 189 | 190 | # Guidance Automation Toolkit 191 | *.gpState 192 | 193 | # ReSharper is a .NET coding add-in 194 | _ReSharper*/ 195 | *.[Rr]e[Ss]harper 196 | *.DotSettings.user 197 | 198 | # TeamCity is a build add-in 199 | _TeamCity* 200 | 201 | # DotCover is a Code Coverage Tool 202 | *.dotCover 203 | 204 | # AxoCover is a Code Coverage Tool 205 | .axoCover/* 206 | !.axoCover/settings.json 207 | 208 | # Coverlet is a free, cross platform Code Coverage Tool 209 | coverage*.json 210 | coverage*.xml 211 | coverage*.info 212 | 213 | # Visual Studio code coverage results 214 | *.coverage 215 | *.coveragexml 216 | 217 | # NCrunch 218 | _NCrunch_* 219 | .*crunch*.local.xml 220 | nCrunchTemp_* 221 | 222 | # MightyMoose 223 | *.mm.* 224 | AutoTest.Net/ 225 | 226 | # Web workbench (sass) 227 | .sass-cache/ 228 | 229 | # Installshield output folder 230 | [Ee]xpress/ 231 | 232 | # DocProject is a documentation generator add-in 233 | DocProject/buildhelp/ 234 | DocProject/Help/*.HxT 235 | DocProject/Help/*.HxC 236 | DocProject/Help/*.hhc 237 | DocProject/Help/*.hhk 238 | DocProject/Help/*.hhp 239 | DocProject/Help/Html2 240 | DocProject/Help/html 241 | 242 | # Click-Once directory 243 | publish/ 244 | 245 | # Publish Web Output 246 | *.[Pp]ublish.xml 247 | *.azurePubxml 248 | # Note: Comment the next line if you want to checkin your web deploy settings, 249 | # but database connection strings (with potential passwords) will be unencrypted 250 | *.pubxml 251 | *.publishproj 252 | 253 | # Microsoft Azure Web App publish settings. Comment the next line if you want to 254 | # checkin your Azure Web App publish settings, but sensitive information contained 255 | # in these scripts will be unencrypted 256 | PublishScripts/ 257 | 258 | # NuGet Packages 259 | *.nupkg 260 | # NuGet Symbol Packages 261 | *.snupkg 262 | # The packages folder can be ignored because of Package Restore 263 | **/[Pp]ackages/* 264 | # except build/, which is used as an MSBuild target. 265 | !**/[Pp]ackages/build/ 266 | # Uncomment if necessary however generally it will be regenerated when needed 267 | #!**/[Pp]ackages/repositories.config 268 | # NuGet v3's project.json files produces more ignorable files 269 | *.nuget.props 270 | *.nuget.targets 271 | 272 | # Microsoft Azure Build Output 273 | csx/ 274 | *.build.csdef 275 | 276 | # Microsoft Azure Emulator 277 | ecf/ 278 | rcf/ 279 | 280 | # Windows Store app package directories and files 281 | AppPackages/ 282 | BundleArtifacts/ 283 | Package.StoreAssociation.xml 284 | _pkginfo.txt 285 | *.appx 286 | *.appxbundle 287 | *.appxupload 288 | 289 | # Visual Studio cache files 290 | # files ending in .cache can be ignored 291 | *.[Cc]ache 292 | # but keep track of directories ending in .cache 293 | !?*.[Cc]ache/ 294 | 295 | # Others 296 | ClientBin/ 297 | ~$* 298 | *~ 299 | *.dbmdl 300 | *.dbproj.schemaview 301 | *.jfm 302 | *.pfx 303 | *.publishsettings 304 | orleans.codegen.cs 305 | 306 | # Including strong name files can present a security risk 307 | # (https://github.com/github/gitignore/pull/2483#issue-259490424) 308 | #*.snk 309 | 310 | # Since there are multiple workflows, uncomment next line to ignore bower_components 311 | # (https://github.com/github/gitignore/pull/1529#issuecomment-104372622) 312 | #bower_components/ 313 | 314 | # RIA/Silverlight projects 315 | Generated_Code/ 316 | 317 | # Backup & report files from converting an old project file 318 | # to a newer Visual Studio version. Backup files are not needed, 319 | # because we have git ;-) 320 | _UpgradeReport_Files/ 321 | Backup*/ 322 | UpgradeLog*.XML 323 | UpgradeLog*.htm 324 | ServiceFabricBackup/ 325 | *.rptproj.bak 326 | 327 | # SQL Server files 328 | *.mdf 329 | *.ldf 330 | *.ndf 331 | 332 | # Business Intelligence projects 333 | *.rdl.data 334 | *.bim.layout 335 | *.bim_*.settings 336 | *.rptproj.rsuser 337 | *- [Bb]ackup.rdl 338 | *- [Bb]ackup ([0-9]).rdl 339 | *- [Bb]ackup ([0-9][0-9]).rdl 340 | 341 | # Microsoft Fakes 342 | FakesAssemblies/ 343 | 344 | # GhostDoc plugin setting file 345 | *.GhostDoc.xml 346 | 347 | # Node.js Tools for Visual Studio 348 | .ntvs_analysis.dat 349 | node_modules/ 350 | 351 | # Visual Studio 6 build log 352 | *.plg 353 | 354 | # Visual Studio 6 workspace options file 355 | *.opt 356 | 357 | # Visual Studio 6 auto-generated workspace file (contains which files were open etc.) 358 | *.vbw 359 | 360 | # Visual Studio LightSwitch build output 361 | **/*.HTMLClient/GeneratedArtifacts 362 | **/*.DesktopClient/GeneratedArtifacts 363 | **/*.DesktopClient/ModelManifest.xml 364 | **/*.Server/GeneratedArtifacts 365 | **/*.Server/ModelManifest.xml 366 | _Pvt_Extensions 367 | 368 | # Paket dependency manager 369 | .paket/paket.exe 370 | paket-files/ 371 | 372 | # FAKE - F# Make 373 | .fake/ 374 | 375 | # CodeRush personal settings 376 | .cr/personal 377 | 378 | # Python Tools for Visual Studio (PTVS) 379 | __pycache__/ 380 | *.pyc 381 | 382 | # Cake - Uncomment if you are using it 383 | # tools/** 384 | # !tools/packages.config 385 | 386 | # Tabs Studio 387 | *.tss 388 | 389 | # Telerik's JustMock configuration file 390 | *.jmconfig 391 | 392 | # BizTalk build output 393 | *.btp.cs 394 | *.btm.cs 395 | *.odx.cs 396 | *.xsd.cs 397 | 398 | # OpenCover UI analysis results 399 | OpenCover/ 400 | 401 | # Azure Stream Analytics local run output 402 | ASALocalRun/ 403 | 404 | # MSBuild Binary and Structured Log 405 | *.binlog 406 | 407 | # NVidia Nsight GPU debugger configuration file 408 | *.nvuser 409 | 410 | # MFractors (Xamarin productivity tool) working folder 411 | .mfractor/ 412 | 413 | # Local History for Visual Studio 414 | .localhistory/ 415 | 416 | # BeatPulse healthcheck temp database 417 | healthchecksdb 418 | 419 | # Backup folder for Package Reference Convert tool in Visual Studio 2017 420 | MigrationBackup/ 421 | 422 | # Ionide (cross platform F# VS Code tools) working folder 423 | .ionide/ 424 | 425 | # Fody - auto-generated XML schema 426 | FodyWeavers.xsd -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 ACRL 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_H 2 | #define PHYSICS2D_H 3 | 4 | #include "physics2d_detector.h" 5 | #include "physics2d_collider.h" 6 | #include "physics2d_tree.h" 7 | 8 | #include "physics2d_ccd.h" 9 | #include "physics2d_common.h" 10 | #include "physics2d_shape.h" 11 | #include "physics2d_algorithm_2d.h" 12 | #include "physics2d_system.h" 13 | #include "physics2d_math.h" 14 | #include "physics2d_linear.h" 15 | #include "physics2d_integrator.h" 16 | #include "physics2d_world.h" 17 | #include "physics2d_joint.h" 18 | #include "physics2d_contact.h" 19 | #include "physics2d_random.h" 20 | #include "physics2d_narrowphase.h" 21 | 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_aabb.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_BROADPHASE_AABB_H 2 | #define PHYSICS2D_BROADPHASE_AABB_H 3 | 4 | #include "physics2d_linear.h" 5 | #include "physics2d_shape.h" 6 | 7 | namespace Physics2D 8 | { 9 | class Body; 10 | 11 | struct PHYSICS2D_API AABB 12 | { 13 | AABB() = default; 14 | AABB(const Vector2& topLeft, const real& boxWidth, const real& boxHeight); 15 | AABB(const Vector2& topLeft, const Vector2& bottomRight); 16 | real width = 0; 17 | real height = 0; 18 | Vector2 position; 19 | inline Vector2 topLeft()const; 20 | inline Vector2 topRight()const; 21 | inline Vector2 bottomLeft()const; 22 | inline Vector2 bottomRight()const; 23 | 24 | inline real minimumX()const; 25 | inline real minimumY()const; 26 | inline real maximumX()const; 27 | inline real maximumY()const; 28 | 29 | bool collide(const AABB& other) const; 30 | void expand(const real& factor); 31 | void scale(const real& factor); 32 | void clear(); 33 | AABB& unite(const AABB& other); 34 | real surfaceArea()const; 35 | real volume()const; 36 | bool isSubset(const AABB& other)const; 37 | bool isEmpty()const; 38 | bool operator==(const AABB& other)const; 39 | bool raycast(const Vector2& start, const Vector2& direction)const; 40 | /// 41 | /// Create AABB from shape. 42 | /// 43 | /// shape source 44 | /// AABB scale factor. Default factor 1 means making tight AABB 45 | /// 46 | static AABB fromShape(const ShapePrimitive& shape, const real& factor = 0); 47 | static AABB fromBody(Body* body, const real& factor = 0); 48 | static AABB fromBox(const Vector2& topLeft, const Vector2& bottomRight); 49 | /// 50 | /// Check if two aabbs are overlapping 51 | /// 52 | /// 53 | /// 54 | /// 55 | static bool collide(const AABB& src, const AABB& target); 56 | /// 57 | /// Return two aabb union result 58 | /// 59 | /// 60 | /// 61 | /// 62 | static AABB unite(const AABB& src, const AABB& target, const real& factor = 0); 63 | /// 64 | /// Check if b is subset of a 65 | /// 66 | /// 67 | /// 68 | /// 69 | static bool isSubset(const AABB& a, const AABB& b); 70 | 71 | static void expand(AABB& aabb, const real& factor = 0.0); 72 | 73 | static bool raycast(const AABB& aabb, const Vector2& start, const Vector2& direction); 74 | 75 | }; 76 | 77 | 78 | } 79 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_algorithm_2d.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_ALGORITHM_GRAPHICS_2D 2 | #define PHYSICS2D_ALGORITHM_GRAPHICS_2D 3 | 4 | #include "physics2d_linear.h" 5 | #include "physics2d_common.h" 6 | namespace Physics2D 7 | { 8 | class PHYSICS2D_API GeometryAlgorithm2D 9 | { 10 | public: 11 | class PHYSICS2D_API Clipper 12 | { 13 | public: 14 | /** 15 | * \brief Sutherland Hodgman Polygon Clipping. All points is stored in counter clock winding.\n 16 | * By convention:\n 17 | * p0 -> p1 -> p2 -> p0 constructs a triangle 18 | * \param polygon 19 | * \param clipRegion 20 | * \return 21 | */ 22 | static Container::Vector sutherlandHodgmentPolygonClipping(const Container::Vector& polygon, const Container::Vector& clipRegion); 23 | }; 24 | 25 | /** 26 | * \brief Check if point a,b,c are collinear using triangle area method 27 | * \param a point a 28 | * \param b point b 29 | * \param c point c 30 | * \return 31 | */ 32 | static bool isCollinear(const Vector2& a, const Vector2& b, const Vector2& c); 33 | /** 34 | * \brief Check if point c is on line segment ab using line projection and set-union method 35 | * \param a end of segment a 36 | * \param b end of segment b 37 | * \param c point c 38 | * \return 39 | */ 40 | static bool isPointOnSegment(const Vector2& a, const Vector2& b, const Vector2& c); 41 | /** 42 | * \brief Check if point c is on line segment ab, given a,b,c is already collinear by calculating cross product 43 | * \param a 44 | * \param b 45 | * \param c 46 | * \param epsilon 47 | * \return 48 | */ 49 | static bool fuzzyIsPointOnSegment(const Vector2& a, const Vector2& b, const Vector2& c, const real& epsilon = Constant::GeometryEpsilon); 50 | static bool fuzzyIsCollinear(const Vector2& a, const Vector2& b, const Vector2& c); 51 | /** 52 | * \brief Calculate intersected point between line ab and line cd.\n 53 | * Notices: overlapping is NOT considered as a kind of intersection situation in this function 54 | * \param a 55 | * \param b 56 | * \param c 57 | * \param d 58 | * \return if there is a actual intersected point. 59 | */ 60 | static std::optional lineSegmentIntersection(const Vector2& a, const Vector2& b, const Vector2& c, const Vector2& d); 61 | /** 62 | * \brief line intersection 63 | * \param p1 64 | * \param p2 65 | * \param q1 66 | * \param q2 67 | * \return 68 | */ 69 | static Vector2 lineIntersection(const Vector2& p1, const Vector2& p2, const Vector2& q1, const Vector2& q2); 70 | /** 71 | * \brief Calculate the center of circum-circle from triangle abc 72 | * \param a 73 | * \param b 74 | * \param c 75 | * \return 76 | */ 77 | static std::optional triangleCircumcenter(const Vector2& a, const Vector2& b, const Vector2& c); 78 | /** 79 | * \brief Calculate the center of inscribed-circle from triangle abc. If a,b,c can not form a triangle, return nothing 80 | * \param a 81 | * \param b 82 | * \param c 83 | * \return 84 | */ 85 | static std::optional triangleIncenter(const Vector2& a, const Vector2& b, const Vector2& c); 86 | /** 87 | * \brief Calculate circum-circle given three points that can form a triangle. If a,b,c can not form a triangle, return nothing 88 | * \param a 89 | * \param b 90 | * \param c 91 | * \return 92 | */ 93 | static std::optional> calculateCircumcircle(const Vector2& a, const Vector2& b, const Vector2& c); 94 | /** 95 | * \brief Calculate inscribed circle given three points that can form a triangle. If a,b,c can not form a triangle, return nothing. 96 | * \param a 97 | * \param b 98 | * \param c 99 | * \return 100 | */ 101 | static std::optional> calculateInscribedCircle(const Vector2& a, const Vector2& b, const Vector2& c); 102 | /** 103 | * \brief Check if a polygon is convex 104 | * \param vertices 105 | * \return 106 | */ 107 | static bool isConvexPolygon(const Container::Vector& vertices); 108 | /** 109 | * \brief Convex hull algorithm: Graham Scan. Given a series of points, find the convex polygon that can contains all of these points. 110 | * \param vertices 111 | * \return 112 | */ 113 | static Container::Vector grahamScan(const Container::Vector& vertices); 114 | /** 115 | * \brief Calculate point on ellipse that is the shortest length to point p(aka projection point). 116 | * \param a 117 | * \param b 118 | * \param p 119 | * \param epsilon 120 | * \return 121 | */ 122 | static Vector2 shortestLengthPointOfEllipse(const real& a, const real& b, const Vector2& p, const real& epsilon = Constant::GeometryEpsilon); 123 | /** 124 | * \brief Calculate the centroid of triangle. 125 | * \param a1 126 | * \param a2 127 | * \param a3 128 | * \return 129 | */ 130 | static Vector2 triangleCentroid(const Vector2& a1, const Vector2& a2, const Vector2& a3); 131 | /** 132 | * \brief Calculate the area of triangle use cross product. 133 | * \param a1 134 | * \param a2 135 | * \param a3 136 | * \return 137 | */ 138 | static real triangleArea(const Vector2& a1, const Vector2& a2, const Vector2& a3); 139 | /** 140 | * \brief Calculate mass center of 'convex' polygon 141 | * \param vertices 142 | * \return 143 | */ 144 | static Vector2 calculateCenter(const Container::Vector& vertices); 145 | static Vector2 calculateCenter(const std::list& vertices); 146 | /** 147 | * \brief Calculate two points on line segment and ellipse respectively. The length of two points is the shortest distance of line segment and ellipse 148 | * \param a major axis a 149 | * \param b minor axis b 150 | * \param p1 line segment point 1 151 | * \param p2 line segment point 2 152 | * \return 153 | */ 154 | static std::tuple shortestLengthLineSegmentEllipse(const real& a, const real& b, const Vector2& p1, const Vector2& p2); 155 | /** 156 | * \brief Calculate point on line segment ab, if point 'p' can cast ray in 'dir' direction on line segment ab. \n 157 | * Algorithm from wikipedia Line-line intersection. 158 | * \param p ray start point 159 | * \param dir ray direction 160 | * \param a line segment point a 161 | * \param b line segment point b 162 | * \return 163 | */ 164 | static std::optional raycast(const Vector2& p, const Vector2& dir, const Vector2& a, const Vector2& b); 165 | static std::optional> raycastAABB(const Vector2& p, const Vector2& dir, const Vector2& topLeft, const Vector2& bottomRight); 166 | static bool isPointOnAABB(const Vector2& p, const Vector2& topLeft, const Vector2& bottomRight); 167 | /** 168 | * \brief Rotate point 'p' around point 'center' by 'angle' degrees 169 | * \param p 170 | * \param center 171 | * \param angle 172 | * \return 173 | */ 174 | static Vector2 rotate(const Vector2& p, const Vector2& center, const real& angle); 175 | /** 176 | * \brief Calculate the projection axis of ellipse in user-define direction. 177 | * \param a 178 | * \param b 179 | * \param direction 180 | * \return the maximum point in ellipse 181 | */ 182 | static Vector2 calculateEllipseProjectionPoint(const real& a, const real& b, const Vector2& direction); 183 | static Vector2 calculateCapsuleProjectionPoint(const real& halfWidth, const real& halfHeight, const Vector2& direction); 184 | static Vector2 calculateSectorProjectionPoint(const real& startRadian, const real& spanRadian, const real& radius, const Vector2& direction); 185 | static bool triangleContainsOrigin(const Vector2& a, const Vector2& b, const Vector2& c); 186 | static bool isPointOnSameSide(const Vector2& edgePoint1, const Vector2& edgePoint2, const Vector2& refPoint, const Vector2 targetPoint); 187 | /** 188 | * \brief calculate normal of line segment with direction of refDirection 189 | * \param edgePoint1 190 | * \param edgePoint2 191 | * \param refDirection 192 | * \return 193 | */ 194 | static Vector2 lineSegmentNormal(const Vector2& edgePoint1, const Vector2& edgePoint2, const Vector2& refDirection); 195 | /** 196 | * \brief calculate point on line segment ab that is the shortest length to point p 197 | * \param a point a 198 | * \param b point b 199 | * \param p target point 200 | * \return 201 | */ 202 | static Vector2 pointToLineSegment(const Vector2& a, const Vector2& b, const Vector2& p); 203 | /** 204 | * \brief ray-ray intersection with no exception check, be sure two rays must can intersect. 205 | * \param p1 206 | * \param dir1 207 | * \param p2 208 | * \param dir2 209 | * \return 210 | */ 211 | static Vector2 rayRayIntersectionUnsafe(const Vector2& p1, const Vector2& dir1, const Vector2& p2, const Vector2& dir2); 212 | }; 213 | } 214 | #endif 215 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_body.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_BODY_H 2 | #define PHYSICS2D_BODY_H 3 | #include "physics2d_aabb.h" 4 | #include "physics2d_math.h" 5 | #include "physics2d_common.h" 6 | #include "physics2d_shape.h" 7 | #include "physics2d_integrator.h" 8 | 9 | #include "physics2d_capsule.h" 10 | #include "physics2d_circle.h" 11 | 12 | #include "physics2d_edge.h" 13 | #include "physics2d_ellipse.h" 14 | #include "physics2d_polygon.h" 15 | #include "physics2d_rectangle.h" 16 | 17 | 18 | namespace Physics2D 19 | { 20 | class PHYSICS2D_API Body 21 | { 22 | public: 23 | struct PHYSICS2D_API BodyPair 24 | { 25 | using BodyPairID = uint64_t; 26 | static BodyPairID generateBodyPairID(Body* bodyA, Body* bodyB); 27 | static BodyPair generateBodyPair(Body* bodyA, Body* bodyB); 28 | BodyPairID pairID; 29 | Body* bodyA; 30 | Body* bodyB; 31 | }; 32 | 33 | enum class PHYSICS2D_API BodyType 34 | { 35 | Kinematic, 36 | Static, 37 | Dynamic, 38 | Bullet 39 | }; 40 | 41 | struct PHYSICS2D_API PhysicsAttribute 42 | { 43 | Vector2 position; 44 | Vector2 velocity; 45 | real rotation = 0; 46 | real angularVelocity = 0; 47 | void step(const real& dt); 48 | }; 49 | 50 | Body() = default; 51 | Vector2& position(); 52 | 53 | Vector2& velocity(); 54 | 55 | real& rotation(); 56 | 57 | real& angularVelocity(); 58 | 59 | Vector2& forces(); 60 | void clearTorque(); 61 | 62 | real& torques(); 63 | 64 | Vector2& lastPosition(); 65 | real& lastRotation(); 66 | uint32_t& sleepCountdown(); 67 | 68 | Shape* shape() const; 69 | void setShape(Shape* shape); 70 | 71 | BodyType type() const; 72 | void setType(const BodyType& type); 73 | 74 | real mass() const; 75 | void setMass(const real& mass); 76 | 77 | real inertia() const; 78 | 79 | AABB aabb(const real& factor = Constant::AABBExpansionFactor) const; 80 | 81 | real friction() const; 82 | void setFriction(const real& friction); 83 | 84 | bool sleep() const; 85 | void setSleep(bool sleep); 86 | 87 | real inverseMass() const; 88 | real inverseInertia() const; 89 | 90 | PhysicsAttribute physicsAttribute() const; 91 | void setPhysicsAttribute(const PhysicsAttribute& info); 92 | 93 | void stepPosition(const real& dt); 94 | 95 | void applyImpulse(const Vector2& impulse, const Vector2& r); 96 | Vector2 toLocalPoint(const Vector2& point) const; 97 | Vector2 toWorldPoint(const Vector2& point) const; 98 | Vector2 toActualPoint(const Vector2& point) const; 99 | 100 | uint32_t id() const; 101 | void setId(const uint32_t& id); 102 | 103 | uint32_t bitmask() const; 104 | void setBitmask(const uint32_t& bitmask); 105 | 106 | real restitution() const; 107 | void setRestitution(const real& restitution); 108 | 109 | real kineticEnergy() const; 110 | 111 | private: 112 | void calcInertia(); 113 | 114 | uint32_t m_id; 115 | uint32_t m_bitmask = 1; 116 | 117 | real m_mass = 0; 118 | real m_inertia = 0; 119 | real m_invMass = 0; 120 | real m_invInertia = 0; 121 | 122 | Vector2 m_position; 123 | Vector2 m_velocity; 124 | real m_rotation = 0; 125 | real m_angularVelocity = 0; 126 | 127 | Vector2 m_lastPosition; 128 | real m_lastRotation = 0; 129 | 130 | Vector2 m_forces; 131 | real m_torques = 0; 132 | 133 | Shape* m_shape; 134 | BodyType m_type = BodyType::Static; 135 | 136 | bool m_sleep = false; 137 | real m_friction = 0.1f; 138 | real m_restitution = 0.0f; 139 | 140 | uint32_t m_sleepCountdown = 0; 141 | }; 142 | } 143 | #endif 144 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_capsule.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_SHAPE_CAPSULE_H 2 | #define PHYSICS2D_SHAPE_CAPSULE_H 3 | #include "physics2d_shape.h" 4 | namespace Physics2D 5 | { 6 | class PHYSICS2D_API Capsule : public Shape 7 | { 8 | public: 9 | Capsule(real width = 0.0f, real height = 0.0f); 10 | bool contains(const Vector2& point, const real& epsilon) override; 11 | void scale(const real& factor) override; 12 | Vector2 center() const override; 13 | void set(real width, real height); 14 | void setWidth(real width); 15 | void setHeight(real height); 16 | real width()const; 17 | real height()const; 18 | real halfWidth()const; 19 | real halfHeight()const; 20 | Vector2 topLeft()const; 21 | Vector2 bottomLeft()const; 22 | Vector2 topRight()const; 23 | Vector2 bottomRight()const; 24 | Container::Vector boxVertices()const; 25 | private: 26 | 27 | real m_halfWidth; 28 | real m_halfHeight; 29 | }; 30 | } 31 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_ccd.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_COLLISION_CCD_H 2 | #define PHYSICS2D_COLLISION_CCD_H 3 | #include "physics2d_detector.h" 4 | 5 | #include "physics2d_body.h" 6 | #include "physics2d_grid.h" 7 | #include "physics2d_tree.h" 8 | 9 | namespace Physics2D 10 | { 11 | /// 12 | /// Continuous Collision Detection 13 | /// This class is implemented by bisection and re-sampling. Both them are costly. 14 | /// 15 | class PHYSICS2D_API CCD 16 | { 17 | public: 18 | struct PHYSICS2D_API AABBShot 19 | { 20 | AABBShot(const AABB& box, const Body::PhysicsAttribute& attr, const real& t) : aabb(box), attribute(attr), 21 | time(t) 22 | { 23 | } 24 | 25 | AABB aabb; 26 | Body::PhysicsAttribute attribute; 27 | real time = 0; 28 | }; 29 | 30 | struct PHYSICS2D_API IndexSection 31 | { 32 | int forward = -1; 33 | int backward = -1; 34 | }; 35 | 36 | struct PHYSICS2D_API CCDPair 37 | { 38 | CCDPair() = default; 39 | 40 | CCDPair(const real& time, Body* target) : toi(time), body(target) 41 | { 42 | } 43 | 44 | real toi = 0.0; 45 | Body* body = nullptr; 46 | }; 47 | 48 | using BroadphaseTrajectory = Container::Vector; 49 | static std::tuple buildTrajectoryAABB(Body* body, const real& dt); 50 | static std::tuple, AABB> buildTrajectoryAABB( 51 | Body* body, const Vector2& target, const real& dt); 52 | static std::optional findBroadphaseRoot(Body* staticBody, 53 | const BroadphaseTrajectory& staticTrajectory, 54 | Body* dynamicBody, 55 | const BroadphaseTrajectory& dynamicTrajectory, 56 | const real& dt); 57 | static std::optional findNarrowphaseRoot(Body* staticBody, const BroadphaseTrajectory& staticTrajectory, 58 | Body* dynamicBody, const BroadphaseTrajectory& dynamicTrajectory, 59 | const IndexSection& index, const real& dt); 60 | 61 | static std::optional> query(Tree& tree, Body* body, const real& dt); 62 | static std::optional> query(UniformGrid& grid, Body* body, const real& dt); 63 | static std::optional earliestTOI(const Container::Vector& pairs, 64 | const real& epsilon = Constant::GeometryEpsilon); 65 | }; 66 | } 67 | #endif 68 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_circle.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_SHAPE_CIRCLE_H 2 | #define PHYSICS2D_SHAPE_CIRCLE_H 3 | #include "physics2d_shape.h" 4 | namespace Physics2D 5 | { 6 | class PHYSICS2D_API Circle : public Shape 7 | { 8 | 9 | public: 10 | Circle(real radius = 0); 11 | 12 | real radius() const; 13 | void setRadius(const real& radius); 14 | void scale(const real& factor) override; 15 | bool contains(const Vector2& point, const real& epsilon = Constant::GeometryEpsilon) override; 16 | Vector2 center()const override; 17 | private: 18 | real m_radius; 19 | }; 20 | } 21 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_collider.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_COLLIDER_H 2 | #define PHYSICS2D_COLLIDER_H 3 | 4 | namespace Physics2D 5 | { 6 | 7 | } 8 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_common.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_COMMON_H 2 | #define PHYSICS2D_COMMON_H 3 | 4 | 5 | #include "cassert" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | 19 | #if defined(_WIN32) 20 | # define PHYSICS2D_API __declspec(dllexport) 21 | #elif defined(__GNUC__) && ((__GNUC__ >= 4) || (__GNUC__ == 3 && __GNUC_MINOR__ >= 3)) 22 | # define PHYSICS2D_API __attribute__((visibility("default"))) 23 | #else 24 | # define PHYSICS2D_API 25 | #endif 26 | 27 | #define SINGLE_PRECISION 28 | 29 | namespace Physics2D 30 | { 31 | using Index = uint32_t; 32 | 33 | namespace Container 34 | { 35 | template 36 | using Vector = std::vector; 37 | 38 | template 39 | using Map = std::map; 40 | } 41 | #ifdef SINGLE_PRECISION 42 | using real = float; 43 | 44 | namespace Constant 45 | { 46 | constexpr unsigned int SimplexMax = 8; 47 | constexpr unsigned int CCDMaxIterations = 15; 48 | constexpr real Epsilon = FLT_EPSILON; 49 | constexpr real Max = FLT_MAX; 50 | constexpr real PositiveMin = FLT_MIN; 51 | constexpr real NegativeMin = -Max; 52 | constexpr real Pi = 3.14159265f; 53 | constexpr real HalfPi = Pi / 2.0f; 54 | constexpr real DoublePi = Pi * 2.0f; 55 | constexpr real ReciprocalOfPi = 0.3183098861f; 56 | constexpr real GeometryEpsilon = 1e-6f; 57 | constexpr real TrignometryEpsilon = 1e-3f; 58 | constexpr real CCDMinVelocity = 100.0f; 59 | constexpr real MaxVelocity = 1000.0f; 60 | constexpr real MaxAngularVelocity = 1000.0f; 61 | constexpr real AABBExpansionFactor = 0.0f; 62 | constexpr real MinLinearVelocity = 1e-4f; 63 | constexpr real MinAngularVelocity = 1e-4f; 64 | constexpr real MinEnergy = 9e-10f; 65 | constexpr size_t SleepCountdown = 32; 66 | constexpr int GJKRetryTimes = 4; 67 | } 68 | #else 69 | using real = double; 70 | namespace Constant 71 | { 72 | constexpr unsigned int SimplexMax = 8; 73 | constexpr real Epsilon = DBL_EPSILON; 74 | constexpr real Max = DBL_MAX; 75 | constexpr real PositiveMin = DBL_MIN; 76 | constexpr real NegativeMin = -Max; 77 | constexpr real Pi = 3.141592653589793238463; 78 | constexpr real HalfPi = Constant::Pi / 2.0; 79 | constexpr real DoublePi = Constant::Pi * 2.0; 80 | constexpr real ReciprocalOfPi = 0.3183098861837907; 81 | constexpr real GeometryEpsilon = 0.0000001; 82 | constexpr real CCDMinVelocity = 100.0; 83 | constexpr real MaxVelocity = 1000.0; 84 | constexpr real MaxAngularVelocity = 1000.0; 85 | constexpr real AABBExpansionFactor = 0.0; 86 | constexpr real MinLinearVelocity = 1e-4; 87 | constexpr real MinAngularVelocity = 1e-4; 88 | constexpr size_t SleepCountdown = 32; 89 | 90 | } 91 | #endif 92 | } 93 | 94 | #endif 95 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_contact.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_CONSTRAINT_CONTACT_H 2 | #define PHYSICS2D_CONSTRAINT_CONTACT_H 3 | #include 4 | 5 | #include "physics2d_body.h" 6 | #include "physics2d_random.h" 7 | #include "physics2d_detector.h" 8 | 9 | namespace Physics2D 10 | { 11 | struct PHYSICS2D_API VelocityConstraintPoint 12 | { 13 | Vector2 localA; 14 | Vector2 localB; 15 | Vector2 ra; 16 | Vector2 rb; 17 | Vector2 va; 18 | Vector2 vb; 19 | Vector2 normal; 20 | Vector2 tangent; 21 | Vector2 velocityBias; 22 | real bias = 0; 23 | real penetration = 0.0f; 24 | real restitution = 0.8f; 25 | real effectiveMassNormal = 0; 26 | real effectiveMassTangent = 0; 27 | real accumulatedNormalImpulse = 0; 28 | real accumulatedTangentImpulse = 0; 29 | }; 30 | 31 | struct PHYSICS2D_API ContactConstraintPoint 32 | { 33 | ContactConstraintPoint() = default; 34 | Body::BodyPair::BodyPairID relation = 0; 35 | real friction = 0.2f; 36 | bool active = true; 37 | Vector2 localA; 38 | Vector2 localB; 39 | Body* bodyA = nullptr; 40 | Body* bodyB = nullptr; 41 | VelocityConstraintPoint vcp; 42 | Matrix2x2 k; 43 | Matrix2x2 normalMass; 44 | }; 45 | 46 | class PHYSICS2D_API ContactMaintainer 47 | { 48 | public: 49 | void clearAll(); 50 | void solveVelocity(real dt); 51 | void solvePosition(real dt); 52 | void add(const Collision& collision); 53 | void prepare(ContactConstraintPoint& ccp, const VertexPair& pair, const Collision& collision); 54 | void clearInactivePoints(); 55 | void deactivateAllPoints(); 56 | real m_maxPenetration = 0.005f; 57 | real m_biasFactor = 0.2f; 58 | bool m_warmStart = true; 59 | bool m_velocityBlockSolver = false; 60 | bool m_positionBlockSolver = false; 61 | Container::Map> m_contactTable; 62 | 63 | private: 64 | }; 65 | } 66 | #endif 67 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_detector.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_DETECTOR_H 2 | #define PHYSICS2D_DETECTOR_H 3 | 4 | #include "physics2d_math.h" 5 | #include "physics2d_shape.h" 6 | #include "physics2d_body.h" 7 | #include "physics2d_narrowphase.h" 8 | 9 | namespace Physics2D 10 | { 11 | struct PHYSICS2D_API Collision 12 | { 13 | bool isColliding = false; 14 | Body* bodyA = nullptr; 15 | Body* bodyB = nullptr; 16 | ContactPair contactList; 17 | Vector2 normal; 18 | real penetration = 0; 19 | }; 20 | 21 | class PHYSICS2D_API Detector 22 | { 23 | public: 24 | static bool collide(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB); 25 | static bool collide(Body* bodyA, Body* bodyB); 26 | static bool collide(const ShapePrimitive& shapeA, Body* bodyB); 27 | static bool collide(Body* bodyA, const ShapePrimitive& shapeB); 28 | 29 | static Collision detect(Body* bodyA, Body* bodyB); 30 | static Collision detect(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB); 31 | static Collision detect(Body* bodyA, const ShapePrimitive& shapeB); 32 | static Collision detect(const ShapePrimitive& shapeA, Body* bodyB); 33 | 34 | static CollisionInfo distance(Body* bodyA, Body* bodyB); 35 | static CollisionInfo distance(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB); 36 | static CollisionInfo distance(Body* bodyA, const ShapePrimitive& shapeB); 37 | static CollisionInfo distance(const ShapePrimitive& shapeA, Body* bodyB); 38 | 39 | private: 40 | }; 41 | } 42 | #endif 43 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_distance_joint.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_DYNAMICS_JOINT_DISTANCE_H 2 | #define PHYSICS2D_DYNAMICS_JOINT_DISTANCE_H 3 | #include "physics2d_joint.h" 4 | namespace Physics2D 5 | { 6 | struct PHYSICS2D_API DistanceJointPrimitive 7 | { 8 | Body* bodyA = nullptr; 9 | Vector2 localPointA; 10 | Vector2 targetPoint; 11 | Vector2 normal; 12 | real biasFactor = 0.3f; 13 | real bias = 0.0f; 14 | real minDistance = 0.0f; 15 | real maxDistance = 0.0f; 16 | real effectiveMass = 0.0f; 17 | real accumulatedImpulse = 0.0f; 18 | }; 19 | struct PHYSICS2D_API DistanceConstraintPrimitive 20 | { 21 | Body* bodyA = nullptr; 22 | Body* bodyB = nullptr; 23 | Vector2 nearestPointA; 24 | Vector2 nearestPointB; 25 | Vector2 ra; 26 | Vector2 rb; 27 | Vector2 bias; 28 | Matrix2x2 effectiveMass; 29 | Vector2 impulse; 30 | real maxForce = 200.0f; 31 | }; 32 | class PHYSICS2D_API DistanceJoint : public Joint 33 | { 34 | public: 35 | DistanceJoint() 36 | { 37 | m_type = JointType::Distance; 38 | } 39 | DistanceJoint(const DistanceJointPrimitive& primitive) : m_primitive(primitive) 40 | { 41 | m_type = JointType::Distance; 42 | } 43 | void set(const DistanceJointPrimitive& primitive) 44 | { 45 | m_primitive = primitive; 46 | } 47 | void prepare(const real& dt) override 48 | { 49 | assert(m_primitive.minDistance <= m_primitive.maxDistance); 50 | Body* bodyA = m_primitive.bodyA; 51 | Vector2 pa = bodyA->toWorldPoint(m_primitive.localPointA); 52 | Vector2 ra = pa - bodyA->position(); 53 | Vector2 pb = m_primitive.targetPoint; 54 | real im_a = m_primitive.bodyA->inverseMass(); 55 | real ii_a = m_primitive.bodyA->inverseInertia(); 56 | Vector2 error = pb - pa; 57 | real length = error.length(); 58 | real c = 0; 59 | 60 | m_primitive.normal = error.normal(); 61 | if (length < m_primitive.minDistance) 62 | { 63 | c = m_primitive.minDistance - length; 64 | m_primitive.normal.negate(); 65 | } 66 | else if (length > m_primitive.maxDistance) 67 | { 68 | c = length - m_primitive.maxDistance; 69 | } 70 | else 71 | { 72 | m_primitive.accumulatedImpulse = 0; 73 | m_primitive.normal.clear(); 74 | m_primitive.bias = 0; 75 | return; 76 | } 77 | if (m_primitive.bodyA->velocity().dot(m_primitive.normal) > 0) 78 | { 79 | m_primitive.accumulatedImpulse = 0; 80 | m_primitive.normal.clear(); 81 | m_primitive.bias = 0; 82 | return; 83 | } 84 | real rn_a = m_primitive.normal.dot(ra); 85 | m_primitive.effectiveMass = 1.0f / (im_a + ii_a * rn_a * rn_a); 86 | m_primitive.bias = m_primitive.biasFactor * c / dt; 87 | 88 | //Vector2 impulse = m_primitive.accumulatedImpulse * m_primitive.normal; 89 | //m_primitive.bodyA->applyImpulse(impulse, ra); 90 | 91 | } 92 | void solveVelocity(const real& dt) override 93 | { 94 | if (m_primitive.bias == 0) 95 | return ; 96 | Vector2 ra = m_primitive.bodyA->toWorldPoint(m_primitive.localPointA) - m_primitive.bodyA->position(); 97 | Vector2 va = m_primitive.bodyA->velocity() + Vector2::crossProduct(m_primitive.bodyA->angularVelocity(), ra); 98 | 99 | Vector2 dv = va; 100 | real jv = m_primitive.normal.dot(dv); 101 | real jvb = -jv + m_primitive.bias; 102 | real lambda_n = m_primitive.effectiveMass * jvb; 103 | 104 | real oldImpulse = m_primitive.accumulatedImpulse; 105 | m_primitive.accumulatedImpulse = Math::max(oldImpulse + lambda_n, 0); 106 | lambda_n = m_primitive.accumulatedImpulse - oldImpulse; 107 | 108 | Vector2 impulse = lambda_n * m_primitive.normal; 109 | m_primitive.bodyA->applyImpulse(impulse, ra); 110 | //m_primitive.bodyA->velocity() += m_primitive.bodyA->inverseMass() * impulse; 111 | //m_primitive.bodyA->angularVelocity() += m_primitive.bodyA->inverseInertia() * ra.cross(impulse); 112 | } 113 | void solvePosition(const real& dt) override 114 | { 115 | 116 | } 117 | 118 | DistanceJointPrimitive& primitive() 119 | { 120 | return m_primitive; 121 | } 122 | private: 123 | real m_factor = 0.4f; 124 | DistanceJointPrimitive m_primitive; 125 | }; 126 | class PHYSICS2D_API DistanceConstraint : public Joint 127 | { 128 | DistanceConstraint() 129 | { 130 | } 131 | DistanceConstraint(const DistanceConstraintPrimitive& primitive) : m_primitive(primitive) 132 | { 133 | } 134 | void set(const DistanceConstraintPrimitive& primitive) 135 | { 136 | m_primitive = primitive; 137 | } 138 | void prepare(const real& dt) override 139 | { 140 | if (m_primitive.bodyA == nullptr || m_primitive.bodyB == nullptr) 141 | return; 142 | 143 | Body* bodyA = m_primitive.bodyB; 144 | Body* bodyB = m_primitive.bodyB; 145 | 146 | real im_a = bodyA->inverseMass(); 147 | real ii_a = bodyA->inverseInertia(); 148 | 149 | real im_b = bodyB->inverseMass(); 150 | real ii_b = bodyB->inverseInertia(); 151 | 152 | m_primitive.ra = m_primitive.nearestPointA - bodyA->position(); 153 | m_primitive.rb = m_primitive.nearestPointB - bodyB->position(); 154 | Vector2& ra = m_primitive.ra; 155 | Vector2& rb = m_primitive.rb; 156 | Vector2 error = m_primitive.nearestPointA - m_primitive.nearestPointB; 157 | 158 | Matrix2x2 k; 159 | k.e11() = im_a + ra.y * ra.y * ii_a + im_b + rb.y * rb.y * ii_b; 160 | k.e12() = -ra.x * ra.y * ii_a - rb.x * rb.y * ii_b; 161 | k.e21() = k.e12(); 162 | k.e22() = im_a + ra.x * ra.x * ii_a + im_b + rb.x * rb.x * ii_b; 163 | m_primitive.bias = error * m_factor; 164 | m_primitive.effectiveMass = k.invert(); 165 | 166 | } 167 | void solveVelocity(const real& dt) override 168 | { 169 | if (m_primitive.bodyA == nullptr || m_primitive.bodyB == nullptr) 170 | return; 171 | Vector2 va = m_primitive.bodyA->velocity() + Vector2::crossProduct(m_primitive.bodyA->angularVelocity(), m_primitive.ra); 172 | Vector2 vb = m_primitive.bodyB->velocity() + Vector2::crossProduct(m_primitive.bodyB->angularVelocity(), m_primitive.rb); 173 | 174 | Vector2 jvb = va - vb; 175 | jvb += m_primitive.bias; 176 | jvb.negate(); 177 | Vector2 J = m_primitive.effectiveMass.multiply(jvb); 178 | Vector2 oldImpulse = m_primitive.impulse; 179 | m_primitive.impulse += J; 180 | real maxImpulse = dt * m_primitive.maxForce; 181 | if (m_primitive.impulse.lengthSquare() > maxImpulse * maxImpulse) 182 | { 183 | m_primitive.impulse.normalize(); 184 | m_primitive.impulse *= maxImpulse; 185 | } 186 | J = m_primitive.impulse - oldImpulse; 187 | m_primitive.bodyA->applyImpulse(J, m_primitive.ra); 188 | m_primitive.bodyB->applyImpulse(-J, m_primitive.rb); 189 | 190 | } 191 | void set(const Vector2& pointA, const Vector2& pointB) 192 | { 193 | m_primitive.nearestPointA = pointA; 194 | m_primitive.nearestPointB = pointB; 195 | } 196 | void solvePosition(const real& dt) override 197 | { 198 | 199 | 200 | } 201 | 202 | DistanceConstraintPrimitive primitive()const 203 | { 204 | return m_primitive; 205 | } 206 | private: 207 | DistanceConstraintPrimitive m_primitive; 208 | real m_factor = 0.1f; 209 | }; 210 | } 211 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_edge.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_SHAPE_EDGE_H 2 | #define PHYSICS2D_SHAPE_EDGE_H 3 | #include "physics2d_shape.h" 4 | namespace Physics2D 5 | { 6 | class PHYSICS2D_API Edge : public Shape 7 | { 8 | 9 | public: 10 | Edge(); 11 | 12 | void set(const Vector2& start, const Vector2& end); 13 | 14 | Vector2 startPoint()const; 15 | void setStartPoint(const Vector2& start); 16 | 17 | Vector2 endPoint()const; 18 | void setEndPoint(const Vector2& end); 19 | void scale(const real& factor) override; 20 | bool contains(const Vector2& point, const real& epsilon = Constant::GeometryEpsilon) override; 21 | Vector2 center()const override; 22 | Vector2 normal()const; 23 | void setNormal(const Vector2& normal); 24 | private: 25 | //0: start 26 | //1: end 27 | Vector2 m_point[2]; 28 | Vector2 m_normal; 29 | }; 30 | } 31 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_ellipse.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_SHAPE_ELLIPSE_H 2 | #define PHYSICS2D_SHAPE_ELLIPSE_H 3 | #include "physics2d_shape.h" 4 | namespace Physics2D 5 | { 6 | class PHYSICS2D_API Ellipse : public Shape 7 | { 8 | 9 | public: 10 | Ellipse(const real& width = 0, const real& height = 0); 11 | void set(const Vector2& leftTop, const Vector2& rightBottom); 12 | void set(const real& width, const real& height); 13 | 14 | real width()const; 15 | void setWidth(const real& width); 16 | 17 | real height()const; 18 | void setHeight(const real& height); 19 | 20 | void scale(const real& factor) override; 21 | bool contains(const Vector2& point, const real& epsilon = Constant::GeometryEpsilon) override; 22 | Vector2 center()const override; 23 | real A()const; 24 | real B()const; 25 | real C()const; 26 | private: 27 | real m_width; 28 | real m_height; 29 | }; 30 | } 31 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_grid.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS_BROADPHASE_GRID_H 2 | #define PHYSICS_BROADPHASE_GRID_H 3 | #include "physics2d_aabb.h" 4 | #include "physics2d_body.h" 5 | #include 6 | #include 7 | 8 | namespace Physics2D 9 | { 10 | //TODO 20220704 11 | //1. Incremental Update Bodies, calculating the different between two cellList 12 | //2. Position combine to u64 and split into two u32 13 | //3. Raycast query bodies 14 | class PHYSICS2D_API UniformGrid 15 | { 16 | public: 17 | UniformGrid(const real& width = 400.0f, const real& height = 400.0f, uint32_t rows = 400, 18 | uint32_t columns = 400); 19 | Container::Vector> generate(); 20 | Container::Vector raycast(const Vector2& p, const Vector2& d); 21 | 22 | void updateAll(); 23 | void update(Body* body); 24 | void insert(Body* body); 25 | void remove(Body* body); 26 | void clearAll(); 27 | Container::Vector query(const AABB& aabb); 28 | 29 | 30 | int rows() const; 31 | void setRows(const int& size); 32 | 33 | int columns() const; 34 | void setColumns(const int& size); 35 | 36 | real width() const; 37 | void setWidth(const real& size); 38 | 39 | real height() const; 40 | void setHeight(const real& size); 41 | 42 | struct Position 43 | { 44 | Position() = default; 45 | 46 | Position(const uint32_t& _x, const uint32_t& _y): x(_x), y(_y) 47 | { 48 | } 49 | 50 | uint32_t x = 0; 51 | uint32_t y = 0; 52 | 53 | bool operator<(const Position& rhs) const 54 | { 55 | if (x < rhs.x) 56 | return true; 57 | if (x == rhs.x) 58 | return y < rhs.y; 59 | return false; 60 | } 61 | 62 | bool operator>(const Position& rhs) const 63 | { 64 | if (x > rhs.x) 65 | return true; 66 | if (x == rhs.x) 67 | return y > rhs.y; 68 | return false; 69 | } 70 | 71 | bool operator==(const Position& rhs) const 72 | { 73 | return x == rhs.x && y == rhs.y; 74 | } 75 | }; 76 | 77 | //AABB query cells 78 | Container::Vector queryCells(const AABB& aabb); 79 | 80 | //ray cast query cells 81 | Container::Vector queryCells(const Vector2& start, const Vector2& direction); 82 | real cellHeight() const; 83 | real cellWidth() const; 84 | 85 | Container::Map> m_cellsToBodies; 86 | Container::Map> m_bodiesToCells; 87 | 88 | void fullUpdate(Body* body); 89 | void incrementalUpdate(Body* body); 90 | 91 | private: 92 | enum class Operation 93 | { 94 | Add, 95 | Delete 96 | }; 97 | 98 | void updateGrid(); 99 | void changeGridSize(); 100 | void updateBodies(); 101 | Container::Vector> compareCellList( 102 | const Container::Vector& oldCellList, const Container::Vector& newCellList); 103 | real m_width = 100.0f; 104 | real m_height = 100.0f; 105 | uint32_t m_rows = 200; 106 | uint32_t m_columns = 200; 107 | 108 | 109 | real m_cellWidth = 0.0f; 110 | real m_cellHeight = 0.0f; 111 | }; 112 | } 113 | 114 | #endif // !PHYSICS_BROADPHASE_GRID_H 115 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_integrator.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_INTEGRATOR_H 2 | #define PHYSICS2D_INTEGRATOR_H 3 | #include "physics2d_common.h" 4 | #include "physics2d_linear.h" 5 | namespace Physics2D 6 | { 7 | class PHYSICS2D_API Integrator 8 | { 9 | public: 10 | struct Timestep 11 | { 12 | real h0 = 0; 13 | real h = 0; 14 | }; 15 | template 16 | struct Value 17 | { 18 | T x0 = 0; // last time 19 | T x = 0; 20 | T dx0 = 0; 21 | T dx = 0; 22 | T ddx = 0; 23 | Timestep step; 24 | }; 25 | 26 | template 27 | class SemiImplicitEuler 28 | { 29 | public: 30 | static void integrate(Value& value, real dt) 31 | { 32 | Value last = value; 33 | value.step.h = dt; 34 | value.dx += value.ddx * dt; 35 | value.x += value.dx * dt; 36 | 37 | //make it last 38 | value.dx0 = last.dx; 39 | value.x0 = last.x; 40 | value.step.h0 = dt; 41 | } 42 | }; 43 | template 44 | class VerletVelocity 45 | { 46 | public: 47 | static void integrate(Value& value, real dt) 48 | { 49 | Value last = value; 50 | value.step.h = dt; 51 | value.x = value.x + value.dx * dt + 0.5 * value.ddx * dt * dt; 52 | value.dx = value.dx + value.ddx * dt; 53 | 54 | value.x0 = last.x; 55 | value.dx0 = last.dx; 56 | value.step.h0 = last.step.h; 57 | } 58 | }; 59 | 60 | template 61 | class VerletPosition 62 | { 63 | public: 64 | 65 | static void integrate(Value& value, real dt) 66 | { 67 | Value last = value; 68 | value.step.h = dt; 69 | value.x = 2 * value.x - value.x0 + value.ddx * dt * dt; 70 | 71 | value.x0 = last.x; 72 | value.dx0 = last.dx; 73 | value.step.h0 = last.step.h; 74 | } 75 | }; 76 | template 77 | class Rk4 78 | { 79 | public: 80 | static void integrate(Value& value, real dt) 81 | { 82 | 83 | } 84 | }; 85 | template 86 | class Adams4 87 | { 88 | public: 89 | static void integrate(Value& value, real dt) 90 | { 91 | 92 | } 93 | }; 94 | }; 95 | } 96 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_joint.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_DYNAMICS_JOINT_JOINT_H 2 | #define PHYSICS2D_DYNAMICS_JOINT_JOINT_H 3 | #include "physics2d_body.h" 4 | namespace Physics2D 5 | { 6 | enum class PHYSICS2D_API JointType 7 | { 8 | Distance, 9 | Point, 10 | Rotation, 11 | Orientation, 12 | Pulley, 13 | Prismatic, 14 | Weld, 15 | Wheel, 16 | Revolute 17 | }; 18 | class PHYSICS2D_API Joint 19 | { 20 | public: 21 | Joint(){} 22 | virtual void prepare(const real& dt) = 0; 23 | virtual void solveVelocity(const real& dt) = 0; 24 | virtual void solvePosition(const real& dt) = 0; 25 | bool active() 26 | { 27 | return m_active; 28 | } 29 | void setActive(bool active) 30 | { 31 | m_active = active; 32 | } 33 | JointType type()const 34 | { 35 | return m_type; 36 | } 37 | uint32_t id()const 38 | { 39 | return m_id; 40 | } 41 | void setId(const uint32_t& id) 42 | { 43 | m_id = id; 44 | } 45 | static real naturalFrequency(real frequency) 46 | { 47 | return Constant::DoublePi * frequency; 48 | } 49 | static real springDampingCoefficient(real mass, real naturalFrequency, real dampingRatio) 50 | { 51 | return dampingRatio * 2.0f * mass * naturalFrequency; 52 | } 53 | static real springStiffness(real mass, real naturalFrequency) 54 | { 55 | return mass * naturalFrequency * naturalFrequency; 56 | } 57 | static real constraintImpulseMixing(real dt, real stiffness, real damping) 58 | { 59 | real cim = dt * (dt * stiffness + damping); 60 | return realEqual(cim, 0.0f) ? 0.0f : 1.0f / cim; 61 | } 62 | static real errorReductionParameter(real dt, real stiffness, real damping) 63 | { 64 | real erp = dt * stiffness + damping; 65 | return realEqual(erp, 0.0f) ? 0.0f : stiffness / erp; 66 | } 67 | protected: 68 | bool m_active = true; 69 | JointType m_type; 70 | uint32_t m_id; 71 | }; 72 | 73 | } 74 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_joints.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_DYNAMICS_JOINTS_H 2 | #define PHYSICS2D_DYNAMICS_JOINTS_H 3 | #include "physics2d_rotation_joint.h" 4 | #include "physics2d_distance_joint.h" 5 | #include "physics2d_point_joint.h" 6 | #include "physics2d_pulley_joint.h" 7 | #include "physics2d_revolute_joint.h" 8 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_linear.h: -------------------------------------------------------------------------------- 1 | #ifndef MATH_LINEAR_H 2 | #define MATH_LINEAR_H 3 | #include "physics2d_vector2.h" 4 | #include "physics2d_vector3.h" 5 | #include "physics2d_vector4.h" 6 | #include "physics2d_matrix2x2.h" 7 | #include "physics2d_matrix3x3.h" 8 | #include "physics2d_matrix4x4.h" 9 | 10 | namespace Physics2D 11 | { 12 | PHYSICS2D_API inline Vector2 operator*(const real& f, const Vector2& v) 13 | { 14 | return v * f; 15 | } 16 | PHYSICS2D_API inline Vector3 operator*(const real& f, const Vector3& v) 17 | { 18 | return v * f; 19 | } 20 | } 21 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_math.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_MATH_H 2 | #define PHYSICS2D_MATH_H 3 | #include "physics2d_common.h" 4 | #include "immintrin.h" 5 | #include "xmmintrin.h" 6 | 7 | namespace Physics2D 8 | { 9 | //basic real utilities 10 | PHYSICS2D_API inline void realSwap(real& lhs, real& rhs) 11 | { 12 | const real temp = lhs; 13 | lhs = rhs; 14 | rhs = temp; 15 | } 16 | 17 | PHYSICS2D_API inline bool fuzzyRealEqual(const real& lhs, const real& rhs, 18 | const real& epsilon = Constant::GeometryEpsilon) 19 | { 20 | return std::fabs(lhs - rhs) < epsilon; 21 | } 22 | 23 | PHYSICS2D_API inline bool realEqual(const real& lhs, const real& rhs) 24 | { 25 | return fuzzyRealEqual(lhs, rhs, Constant::GeometryEpsilon); 26 | } 27 | 28 | class Math 29 | { 30 | public: 31 | //trigonometric function 32 | PHYSICS2D_API static real abs(const real& x) 33 | { 34 | return std::fabs(x); 35 | } 36 | 37 | PHYSICS2D_API static real sinx(const real& x) 38 | { 39 | return std::sin(x); 40 | } 41 | 42 | PHYSICS2D_API static real cosx(const real& x) 43 | { 44 | return std::cos(x); 45 | } 46 | 47 | PHYSICS2D_API static real tanx(const real& x) 48 | { 49 | return std::tan(x); 50 | } 51 | 52 | PHYSICS2D_API static real arcsinx(const real& x) 53 | { 54 | return std::asin(x); 55 | } 56 | 57 | PHYSICS2D_API static real arccosx(const real& x) 58 | { 59 | return std::acos(x); 60 | } 61 | 62 | PHYSICS2D_API static real arctanx(const real& y, const real& x) 63 | { 64 | return std::atan2(y, x); 65 | } 66 | 67 | PHYSICS2D_API static real max(const real& a, const real& b) 68 | { 69 | return std::max(a, b); 70 | } 71 | 72 | PHYSICS2D_API static real min(const real& a, const real& b) 73 | { 74 | return std::min(a, b); 75 | } 76 | 77 | PHYSICS2D_API static real tripleMin(const real& a, const real& b, const real& c) 78 | { 79 | return std::min(std::min(a, b), c); 80 | } 81 | 82 | PHYSICS2D_API static real tripleMax(const real& a, const real& b, const real& c) 83 | { 84 | return std::max(std::max(a, b), c); 85 | } 86 | 87 | PHYSICS2D_API static real absMax(const real& a, const real& b) 88 | { 89 | return std::max(std::fabs(a), std::fabs(b)); 90 | } 91 | 92 | PHYSICS2D_API static real absMin(const real& a, const real& b) 93 | { 94 | return std::min(std::fabs(a), std::fabs(b)); 95 | } 96 | 97 | PHYSICS2D_API static real sqrt(const real& x) 98 | { 99 | return std::sqrt(x); 100 | } 101 | 102 | PHYSICS2D_API static real pow(const real& x, const real& e) 103 | { 104 | return std::pow(x, e); 105 | } 106 | 107 | //other 108 | PHYSICS2D_API static bool sameSign(const real& a, const real& b) 109 | { 110 | return a >= 0 && b >= 0 || a <= 0 && b <= 0; 111 | } 112 | 113 | PHYSICS2D_API static bool sameSign(const real& a, const real& b, const real& c) 114 | { 115 | return a >= 0 && b >= 0 && c >= 0 || a <= 0 && b <= 0 && c <= 0; 116 | } 117 | 118 | PHYSICS2D_API static bool sameSignStrict(const real& a, const real& b) 119 | { 120 | return a > 0 && b > 0 || a < 0 && b < 0; 121 | } 122 | 123 | PHYSICS2D_API static bool sameSignStrict(const real& a, const real& b, const real& c) 124 | { 125 | return a > 0 && b > 0 && c > 0 || a < 0 && b < 0 && c < 0; 126 | } 127 | 128 | PHYSICS2D_API static int8_t sign(const real& num) 129 | { 130 | return num > 0 ? 1 : -1; 131 | } 132 | 133 | PHYSICS2D_API static bool isInRange(const real& value, const real& low, const real& high, 134 | const real& epsilon = Constant::GeometryEpsilon) 135 | { 136 | return value >= low - epsilon && value <= high + epsilon; 137 | } 138 | 139 | PHYSICS2D_API static bool fuzzyIsInRange(const real& value, const real& low, const real& high, 140 | const real& epsilon = Constant::GeometryEpsilon) 141 | { 142 | return value >= low - epsilon && value <= high + epsilon || value <= low + epsilon && low >= high - epsilon; 143 | } 144 | 145 | PHYSICS2D_API static real clamp(const real& num, const real& low, const real& high) 146 | { 147 | return std::clamp(num, low, high); 148 | } 149 | 150 | PHYSICS2D_API static size_t clamp(const size_t& num, const size_t& low, const size_t& high) 151 | { 152 | if (num < low) 153 | return low; 154 | if (num > high) 155 | return high; 156 | return num; 157 | } 158 | 159 | PHYSICS2D_API static real degreeToRadian(const real& angle) 160 | { 161 | return angle * (Constant::Pi / 180.0f); 162 | } 163 | 164 | PHYSICS2D_API static real radianToDegree(const real& radian) 165 | { 166 | return radian * (180.0f / Constant::Pi); 167 | } 168 | 169 | template 170 | PHYSICS2D_API static T fastInverseSqrt(T x) 171 | { 172 | using Tint = std::conditional_t; 173 | T y = x; 174 | T x2 = y * 0.5f; 175 | Tint i = *(Tint*)&y; 176 | i = (sizeof(T) == 8 ? 0x5fe6eb50c7b537a9 : 0x5f3759df) - (i >> 1); 177 | y = *(T*)&i; 178 | for (size_t k = 0; k <= iterations; k++) 179 | y = y * (1.5f - (x2 * y * y)); 180 | return y; 181 | } 182 | }; 183 | } 184 | #endif 185 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_matrix2x2.h: -------------------------------------------------------------------------------- 1 | #ifndef MATH_LINEAR_MATRIX2X2_H 2 | #define MATH_LINEAR_MATRIX2X2_H 3 | #include "physics2d_vector2.h" 4 | #include "physics2d_math.h" 5 | namespace Physics2D 6 | { 7 | 8 | struct PHYSICS2D_API Matrix2x2 9 | { 10 | Matrix2x2() = default; 11 | Matrix2x2(const real& radian); 12 | Matrix2x2(const Matrix2x2& mat); 13 | Matrix2x2(const Vector2& col1, const Vector2& col2); 14 | Matrix2x2(const real& col1_x, const real& col1_y, const real& col2_x, const real& col2_y); 15 | Matrix2x2(Matrix2x2&& other) = default; 16 | 17 | Matrix2x2& operator=(const Matrix2x2& rhs); 18 | Matrix2x2& operator+=(const Matrix2x2& rhs); 19 | Matrix2x2& operator-=(const Matrix2x2& rhs); 20 | Matrix2x2& operator*=(const real& factor); 21 | Matrix2x2& operator/=(const real& factor); 22 | Matrix2x2 operator+(const Matrix2x2& rhs)const; 23 | Matrix2x2 operator-(const Matrix2x2& rhs)const; 24 | 25 | Vector2 row1()const; 26 | Vector2 row2()const; 27 | 28 | real& e11(); 29 | real& e12(); 30 | 31 | real& e21(); 32 | real& e22(); 33 | 34 | real determinant()const; 35 | Matrix2x2& transpose(); 36 | Matrix2x2& invert(); 37 | Matrix2x2& multiply(const Matrix2x2& rhs); 38 | Vector2 multiply(const Vector2& rhs)const; 39 | 40 | Matrix2x2& clear(); 41 | Matrix2x2& set(const real& col1_x, const real& col1_y, const real& col2_x, const real& col2_y); 42 | Matrix2x2& set(const Vector2& col1, const Vector2& col2); 43 | Matrix2x2& set(const Matrix2x2& other); 44 | Matrix2x2& set(const real& radian); 45 | 46 | static Matrix2x2 skewSymmetricMatrix(const Vector2& r); 47 | static Matrix2x2 identityMatrix(); 48 | static Vector2 multiply(const Matrix2x2& lhs, const Vector2& rhs); 49 | static Matrix2x2 multiply(const Matrix2x2& lhs, const Matrix2x2& rhs); 50 | static real determinant(const Matrix2x2& mat); 51 | static bool invert(Matrix2x2& mat); 52 | 53 | Vector2 column1; 54 | Vector2 column2; 55 | }; 56 | } 57 | #endif 58 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_matrix3x3.h: -------------------------------------------------------------------------------- 1 | #ifndef MATH_LINEAR_MATRIX3X3_H 2 | #define MATH_LINEAR_MATRIX3X3_H 3 | #include "physics2d_common.h" 4 | #include "physics2d_math.h" 5 | #include "physics2d_vector2.h" 6 | #include "physics2d_vector3.h" 7 | namespace Physics2D 8 | { 9 | 10 | struct PHYSICS2D_API Matrix3x3 11 | { 12 | Matrix3x3() = default; 13 | Matrix3x3(const Matrix3x3& mat); 14 | Matrix3x3(const Vector3& col1, const Vector3& col2, const Vector3& col3); 15 | Matrix3x3(const real& col1_x, const real& col1_y, const real& col1_z, 16 | const real& col2_x, const real& col2_y, const real& col2_z, 17 | const real& col3_x, const real& col3_y, const real& col3_z); 18 | Matrix3x3(Matrix3x3&& other) = default; 19 | 20 | Matrix3x3& operator=(const Matrix3x3& rhs); 21 | Matrix3x3& operator+=(const Matrix3x3& rhs); 22 | Matrix3x3& operator-=(const Matrix3x3& rhs); 23 | Matrix3x3& operator*=(const real& factor); 24 | Matrix3x3& operator/=(const real& factor); 25 | 26 | Vector3 row1()const; 27 | Vector3 row2()const; 28 | Vector3 row3()const; 29 | 30 | real& e11(); 31 | real& e12(); 32 | real& e13(); 33 | 34 | real& e21(); 35 | real& e22(); 36 | real& e23(); 37 | 38 | real& e31(); 39 | real& e32(); 40 | real& e33(); 41 | 42 | Matrix3x3& set(const real& col1_x, const real& col1_y, const real& col1_z, 43 | const real& col2_x, const real& col2_y, const real& col2_z, 44 | const real& col3_x, const real& col3_y, const real& col3_z); 45 | Matrix3x3& set(const Vector3& col1, const Vector3& col2, const Vector3& col3); 46 | Matrix3x3& set(const Matrix3x3& other); 47 | Matrix3x3& clear(); 48 | 49 | Vector3 multiply(const Vector3& rhs)const; 50 | Matrix3x3& multiply(const Matrix3x3& rhs); 51 | real determinant()const; 52 | Matrix3x3& transpose(); 53 | Matrix3x3& invert(); 54 | 55 | static Matrix3x3 skewSymmetricMatrix(const Vector3& v); 56 | static Matrix3x3 identityMatrix(); 57 | static Matrix3x3 multiply(const Matrix3x3& lhs, const Matrix3x3& rhs); 58 | static Vector3 multiply(const Matrix3x3& lhs, const Vector3& rhs); 59 | static real determinant(const Matrix3x3& mat); 60 | static bool invert(Matrix3x3& mat); 61 | 62 | Vector3 column1; 63 | Vector3 column2; 64 | Vector3 column3; 65 | }; 66 | } 67 | #endif 68 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_matrix4x4.h: -------------------------------------------------------------------------------- 1 | #ifndef MATH_LINEAR_MATRIX4X4_H 2 | #define MATH_LINEAR_MATRIX4X4_H 3 | #include "physics2d_common.h" 4 | #include "physics2d_math.h" 5 | #include "physics2d_matrix3x3.h" 6 | #include "physics2d_vector4.h" 7 | namespace Physics2D 8 | { 9 | struct PHYSICS2D_API Matrix4x4 10 | { 11 | Matrix4x4() = default; 12 | Matrix4x4(const Vector4& col1, const Vector4& col2, const Vector4& col3, const Vector4& col4); 13 | 14 | Matrix4x4(const real& col1_x, const real& col1_y, const real& col1_z, const real& col1_w, 15 | const real& col2_x, const real& col2_y, const real& col2_z, const real& col2_w, 16 | const real& col3_x, const real& col3_y, const real& col3_z, const real& col3_w, 17 | const real& col4_x, const real& col4_y, const real& col4_z, const real& col4_w); 18 | 19 | Matrix4x4(const Matrix3x3& mat); 20 | Matrix4x4(const Matrix4x4& mat) = default; 21 | 22 | Matrix4x4(Matrix4x4&& other) = default; 23 | 24 | Matrix4x4& operator=(const Matrix3x3& rhs); 25 | Matrix4x4& operator=(const Matrix4x4& rhs); 26 | 27 | Matrix4x4& operator+=(const Matrix4x4& rhs); 28 | Matrix4x4& operator-=(const Matrix4x4& rhs); 29 | Matrix4x4& operator*=(const real& factor); 30 | Matrix4x4& operator/=(const real& factor); 31 | 32 | Vector4 row1()const; 33 | Vector4 row2()const; 34 | Vector4 row3()const; 35 | Vector4 row4()const; 36 | 37 | real& e11(); 38 | real& e12(); 39 | real& e13(); 40 | real& e14(); 41 | 42 | real& e21(); 43 | real& e22(); 44 | real& e23(); 45 | real& e24(); 46 | 47 | real& e31(); 48 | real& e32(); 49 | real& e33(); 50 | real& e34(); 51 | 52 | real& e41(); 53 | real& e42(); 54 | real& e43(); 55 | real& e44(); 56 | 57 | Matrix4x4& set(const real& col1_x, const real& col1_y, const real& col1_z, const real& col1_w, 58 | const real& col2_x, const real& col2_y, const real& col2_z, const real& col2_w, 59 | const real& col3_x, const real& col3_y, const real& col3_z, const real& col3_w, 60 | const real& col4_x, const real& col4_y, const real& col4_z, const real& col4_w); 61 | 62 | Matrix4x4& set(const Vector4& col1, const Vector4& col2, const Vector4& col3, const Vector4& col4); 63 | Matrix4x4& set(const Matrix4x4& other); 64 | Matrix4x4& set(const Matrix3x3& other); 65 | 66 | Matrix4x4& clear(); 67 | 68 | Vector4 multiply(const Vector4& rhs)const; 69 | Matrix4x4& multiply(const Matrix4x4& rhs); 70 | real determinant()const; 71 | Matrix4x4& transpose(); 72 | Matrix4x4& invert(); 73 | 74 | static Matrix4x4 identityMatrix(); 75 | static Matrix4x4 multiply(const Matrix4x4& lhs, const Matrix4x4& rhs); 76 | static Vector4 multiply(const Matrix4x4& lhs, const Vector4& rhs); 77 | static real determinant(const Matrix4x4& mat); 78 | static bool invert(Matrix4x4& mat); 79 | 80 | Vector4 column1; 81 | Vector4 column2; 82 | Vector4 column3; 83 | Vector4 column4; 84 | }; 85 | } 86 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_narrowphase.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_NARROWPHASE_H 2 | #define PHYSICS2D_NARROWPHASE_H 3 | #include "physics2d_common.h" 4 | #include "physics2d_shape.h" 5 | #include "physics2d_simplex.h" 6 | 7 | #include "physics2d_capsule.h" 8 | #include "physics2d_circle.h" 9 | 10 | #include "physics2d_edge.h" 11 | #include "physics2d_ellipse.h" 12 | #include "physics2d_polygon.h" 13 | #include "physics2d_rectangle.h" 14 | 15 | 16 | 17 | namespace Physics2D 18 | { 19 | struct PHYSICS2D_API SimplexVertexWithOriginDistance 20 | { 21 | SimplexVertex vertex; 22 | real distance = 0.0f; 23 | }; 24 | 25 | struct PHYSICS2D_API Feature 26 | { 27 | //circle and ellipse, use index 0 28 | //edge use index 0 and 1 29 | Vector2 vertex[2]; 30 | Index index[2] = {UINT32_MAX, UINT32_MAX}; 31 | }; 32 | 33 | struct PHYSICS2D_API ClipVertex 34 | { 35 | Vector2 vertex; 36 | bool isClip = false; 37 | Vector2 clipperVertex; 38 | bool isFinalValid = false; 39 | }; 40 | 41 | struct PHYSICS2D_API ContactPair 42 | { 43 | //contact pair1: 44 | // points[0]: pointA 45 | // points[1]: pointB 46 | 47 | //if there is second contact pair: 48 | // points[2]: pointA 49 | // points[3]: pointB 50 | std::array points; 51 | std::array ids; 52 | uint32_t count = 0; 53 | 54 | void addContact(const Vector2& pointA, const Vector2& pointB) 55 | { 56 | assert(count <= 4); 57 | points[count++] = pointA; 58 | points[count++] = pointB; 59 | } 60 | }; 61 | 62 | 63 | struct PHYSICS2D_API VertexPair 64 | { 65 | VertexPair() = default; 66 | Vector2 pointA; 67 | Vector2 pointB; 68 | 69 | bool isEmpty() const 70 | { 71 | return pointA.fuzzyEqual({0, 0}) && pointB.fuzzyEqual({0, 0}); 72 | } 73 | 74 | bool operator==(const VertexPair& other) const 75 | { 76 | return (other.pointA.fuzzyEqual(this->pointA) && other.pointB.fuzzyEqual(this->pointB)) 77 | || (other.pointB.fuzzyEqual(this->pointA) && other.pointA.fuzzyEqual(this->pointB)); 78 | } 79 | }; 80 | 81 | struct PHYSICS2D_API CollisionInfo 82 | { 83 | Vector2 normal; 84 | real penetration = 0; 85 | Simplex simplex; 86 | VertexPair pair; 87 | //[Debug] 88 | Simplex originalSimplex; 89 | std::list polytope; 90 | }; 91 | 92 | class PHYSICS2D_API Narrowphase 93 | { 94 | public: 95 | static Simplex gjk(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB, const size_t& iteration = 12); 96 | static CollisionInfo epa(const Simplex& simplex, const ShapePrimitive& shapeA, const ShapePrimitive& shapeB, 97 | const size_t& iteration = 12, const real& epsilon = Constant::GeometryEpsilon); 98 | static SimplexVertex support(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB, 99 | const Vector2& direction); 100 | static std::pair findFurthestPoint(const ShapePrimitive& shape, const Vector2& direction); 101 | static Vector2 findDirectionByEdge(const SimplexVertex& v1, const SimplexVertex& v2, bool pointToOrigin); 102 | static std::pair findFurthestPoint(const Container::Vector& vertices, 103 | const Vector2& direction); 104 | static ContactPair generateContacts(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB, 105 | CollisionInfo& info); 106 | 107 | static CollisionInfo gjkDistance(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB, 108 | const size_t& iteration = 10); 109 | 110 | private: 111 | static void reconstructSimplexByVoronoi(Simplex& simplex); 112 | 113 | static bool perturbSimplex(Simplex& simplex, const ShapePrimitive& shapeA, const ShapePrimitive& shapeB, 114 | const Vector2& dir); 115 | 116 | static Feature findFeatures(const Simplex& simplex, const Vector2& normal, const ShapePrimitive& shape, 117 | const Index& AorB); 118 | 119 | static ContactPair clipTwoEdge(const Vector2& va1, const Vector2& va2, const Vector2& vb1, const Vector2& vb2, 120 | CollisionInfo& info); 121 | 122 | static ContactPair clipIncidentEdge(std::array& incEdge, std::array refEdge, 123 | const Vector2& normal, bool swap); 124 | 125 | static ContactPair clipPolygonPolygon(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB, 126 | const Feature& featureA, const Feature& featureB, CollisionInfo& info); 127 | static ContactPair clipPolygonEdge(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB, 128 | const Feature& featureA, const Feature& featureB, CollisionInfo& info); 129 | static ContactPair clipPolygonCapsule(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB, 130 | const Feature& featureA, const Feature& featureB, CollisionInfo& info); 131 | static ContactPair clipPolygonRound(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB, 132 | const Feature& featureA, const Feature& featureB, CollisionInfo& info); 133 | 134 | static ContactPair clipEdgeCapsule(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB, 135 | const Feature& featureA, const Feature& featureB, CollisionInfo& info); 136 | static ContactPair clipEdgeRound(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB, 137 | const Feature& featureA, const Feature& featureB, CollisionInfo& info); 138 | 139 | static ContactPair clipCapsuleCapsule(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB, 140 | const Feature& featureA, const Feature& featureB, CollisionInfo& info); 141 | static ContactPair clipCapsuleRound(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB, 142 | const Feature& featureA, const Feature& featureB, CollisionInfo& info); 143 | 144 | static ContactPair clipRoundRound(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB, 145 | const Feature& featureA, const Feature& featureB, CollisionInfo& info); 146 | 147 | static ContactPair clipEdgeVertex(const Vector2& va1, const Vector2& va2, const Vector2& vb, 148 | CollisionInfo& info); 149 | 150 | static void polytopeIterNext(std::list::iterator& targetIter, 151 | std::list& list); 152 | static void polytopeIterPrev(std::list::iterator& targetIter, 153 | std::list& list); 154 | 155 | static void buildPolytopeFromSimplex(std::list& polytope, 156 | const Simplex& simplex); 157 | }; 158 | } 159 | #endif 160 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_point_joint.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_DYNAMICS_JOINT_POINT_H 2 | #define PHYSICS2D_DYNAMICS_JOINT_POINT_H 3 | #include "physics2d_joint.h" 4 | 5 | namespace Physics2D 6 | { 7 | struct PHYSICS2D_API PointJointPrimitive 8 | { 9 | Body* bodyA = nullptr; 10 | Vector2 localPointA; 11 | Vector2 targetPoint; 12 | Vector2 normal; 13 | 14 | real damping = 0.0; 15 | real stiffness = 0.0; 16 | real frequency = 10; 17 | real maxForce = 1000; 18 | real dampingRatio = 0.707f; 19 | real gamma = 0.0; 20 | Vector2 bias; 21 | Matrix2x2 effectiveMass; 22 | Vector2 accumulatedImpulse; 23 | 24 | void clear() 25 | { 26 | accumulatedImpulse.clear(); 27 | effectiveMass.clear(); 28 | bias.clear(); 29 | } 30 | }; 31 | 32 | class PHYSICS2D_API PointJoint : public Joint 33 | { 34 | public: 35 | PointJoint() 36 | { 37 | m_type = JointType::Point; 38 | } 39 | 40 | PointJoint(const PointJointPrimitive& prim) : m_primitive(prim) 41 | { 42 | m_type = JointType::Point; 43 | } 44 | 45 | void set(const PointJointPrimitive& prim) 46 | { 47 | m_primitive = prim; 48 | } 49 | 50 | void prepare(const real& dt) override 51 | { 52 | if (m_primitive.bodyA == nullptr) 53 | return; 54 | Body* bodyA = m_primitive.bodyA; 55 | 56 | real m_a = bodyA->mass(); 57 | real im_a = bodyA->inverseMass(); 58 | real ii_a = bodyA->inverseInertia(); 59 | if (m_primitive.frequency > 0.0) 60 | { 61 | real nf = naturalFrequency(m_primitive.frequency); 62 | m_primitive.stiffness = springStiffness(m_a, nf); 63 | m_primitive.damping = springDampingCoefficient(m_a, nf, m_primitive.dampingRatio); 64 | } 65 | else 66 | { 67 | m_primitive.stiffness = 0.0; 68 | m_primitive.damping = 0.0; 69 | } 70 | m_primitive.gamma = constraintImpulseMixing(dt, m_primitive.stiffness, m_primitive.damping); 71 | real erp = errorReductionParameter(dt, m_primitive.stiffness, m_primitive.damping); 72 | 73 | 74 | Vector2 pa = bodyA->toWorldPoint(m_primitive.localPointA); 75 | Vector2 ra = pa - bodyA->position(); 76 | Vector2 pb = m_primitive.targetPoint; 77 | 78 | m_primitive.bias = (pa - pb) * erp; 79 | Matrix2x2 k; 80 | k.e11() = im_a + ra.y * ra.y * ii_a; 81 | k.e12() = -ra.x * ra.y * ii_a; 82 | k.e21() = k.e12(); 83 | k.e22() = im_a + ra.x * ra.x * ii_a; 84 | 85 | k.e11() += m_primitive.gamma; 86 | k.e22() += m_primitive.gamma; 87 | 88 | m_primitive.effectiveMass = k.invert(); 89 | //warmstart 90 | //m_primitive.impulse *= dt / dt; 91 | 92 | bodyA->applyImpulse(m_primitive.accumulatedImpulse, ra); 93 | } 94 | 95 | void solveVelocity(const real& dt) override 96 | { 97 | if (m_primitive.bodyA == nullptr) 98 | return; 99 | Vector2 ra = m_primitive.bodyA->toWorldPoint(m_primitive.localPointA) - m_primitive.bodyA->position(); 100 | Vector2 va = m_primitive.bodyA->velocity() + 101 | Vector2::crossProduct(m_primitive.bodyA->angularVelocity(), ra); 102 | Vector2 jvb = va; 103 | jvb += m_primitive.bias; 104 | jvb += m_primitive.accumulatedImpulse * m_primitive.gamma; 105 | jvb.negate(); 106 | 107 | Vector2 J = m_primitive.effectiveMass.multiply(jvb); 108 | Vector2 oldImpulse = m_primitive.accumulatedImpulse; 109 | m_primitive.accumulatedImpulse += J; 110 | real maxImpulse = dt * m_primitive.maxForce; 111 | if (m_primitive.accumulatedImpulse.lengthSquare() > maxImpulse * maxImpulse) 112 | { 113 | m_primitive.accumulatedImpulse.normalize(); 114 | m_primitive.accumulatedImpulse *= maxImpulse; 115 | } 116 | J = m_primitive.accumulatedImpulse - oldImpulse; 117 | 118 | m_primitive.bodyA->applyImpulse(J, ra); 119 | } 120 | 121 | void solvePosition(const real& dt) override 122 | { 123 | } 124 | 125 | PointJointPrimitive& primitive() 126 | { 127 | return m_primitive; 128 | } 129 | 130 | private: 131 | PointJointPrimitive m_primitive; 132 | real m_factor = 0.22f; 133 | }; 134 | } 135 | #endif 136 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_polygon.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_SHAPE_POLYGON_H 2 | #define PHYSICS2D_SHAPE_POLYGON_H 3 | #include "physics2d_shape.h" 4 | namespace Physics2D 5 | { 6 | class PHYSICS2D_API Polygon : public Shape 7 | { 8 | 9 | public: 10 | Polygon(); 11 | 12 | const Container::Vector& vertices() const; 13 | void append(const std::initializer_list& vertices); 14 | void append(const Vector2& vertex); 15 | Vector2 center()const override; 16 | void scale(const real& factor) override; 17 | bool contains(const Vector2& point, const real& epsilon = Constant::GeometryEpsilon) override; 18 | protected: 19 | Container::Vector m_vertices; 20 | void updateVertices(); 21 | }; 22 | } 23 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_pulley_joint.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_DYNAMICS_JOINT_PULLEY_H 2 | #define PHYSICS2D_DYNAMICS_JOINT_PULLEY_H 3 | 4 | namespace Physics2D 5 | { 6 | struct PHYSICS2D_API PulleyJointPrimitive 7 | { 8 | 9 | }; 10 | class PHYSICS2D_API PulleyJoint : public Joint 11 | { 12 | public: 13 | PulleyJoint() 14 | { 15 | m_type = JointType::Pulley; 16 | } 17 | 18 | PulleyJoint(const PulleyJointPrimitive& primitive) 19 | { 20 | m_type = JointType::Pulley; 21 | m_primitive = primitive; 22 | } 23 | void set(const PulleyJointPrimitive& primitive) 24 | { 25 | m_primitive = primitive; 26 | } 27 | void prepare(const real& dt) override 28 | { 29 | 30 | } 31 | void solveVelocity(const real& dt) override 32 | { 33 | 34 | } 35 | void solvePosition(const real& dt) override 36 | { 37 | 38 | } 39 | private: 40 | PulleyJointPrimitive m_primitive; 41 | }; 42 | } 43 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_quaternion.h: -------------------------------------------------------------------------------- 1 | #ifndef MATH_LINEAR_QUATERNION_H 2 | #define MATH_LINEAR_QUATERNION_H 3 | #include "physics2d_common.h" 4 | #include "physics2d_vector4.h" 5 | namespace Physics2D 6 | { 7 | struct PHYSICS2D_API Quaternion 8 | { 9 | Quaternion(const real& s, const real& i, const real& j, const real& k); 10 | Quaternion(const Vector4& vec4); 11 | Quaternion(const real& s, const Vector3& vec3); 12 | Quaternion(const Quaternion& copy) = default; 13 | Quaternion(Quaternion&& copy) = default; 14 | real s; 15 | Vector3 v; 16 | }; 17 | } 18 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_random.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS_RANDOM_H 2 | #define PHYSICS_RANDOM_H 3 | 4 | #include 5 | 6 | #include "physics2d_common.h" 7 | 8 | namespace Physics2D 9 | { 10 | static std::random_device randomDevice; 11 | static std::mt19937_64 randomEngine(randomDevice()); 12 | static std::uniform_int_distribution uniformDistribution(10000, 99999); 13 | static std::vector m_emptyList; 14 | static uint32_t m_startId = 1000; 15 | class PHYSICS2D_API RandomGenerator 16 | { 17 | public: 18 | static real uniform(const real& min, const real& max) 19 | { 20 | std::uniform_real_distribution distribution(min, max); 21 | return distribution(randomEngine); 22 | } 23 | static unsigned int unique() 24 | { 25 | //return uniformDistribution(randomEngine); 26 | if(!m_emptyList.empty()) 27 | { 28 | auto result = m_emptyList.back(); 29 | m_emptyList.pop_back(); 30 | return result; 31 | } 32 | return m_startId++; 33 | } 34 | 35 | static void pop(uint32_t id) 36 | { 37 | m_emptyList.push_back(id); 38 | } 39 | }; 40 | } 41 | 42 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_rectangle.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_SHAPE_RECTANGLE_H 2 | #define PHYSICS2D_SHAPE_RECTANGLE_H 3 | #include "physics2d_polygon.h" 4 | namespace Physics2D 5 | { 6 | class PHYSICS2D_API Rectangle : public Polygon 7 | { 8 | 9 | public: 10 | Rectangle(const real& width = 0, const real& height = 0); 11 | void set(const real& width, const real& height); 12 | 13 | real width()const; 14 | void setWidth(const real& width); 15 | 16 | real height()const; 17 | void setHeight(const real& height); 18 | 19 | void scale(const real& factor) override; 20 | bool contains(const Vector2& point, const real& epsilon = Constant::GeometryEpsilon) override; 21 | private: 22 | void calcVertices(); 23 | real m_width; 24 | real m_height; 25 | }; 26 | } 27 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_revolute_joint.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_DYNAMICS_JOINT_REVOLUTE_H 2 | #define PHYSICS2D_DYNAMICS_JOINT_REVOLUTE_H 3 | 4 | namespace Physics2D 5 | { 6 | struct PHYSICS2D_API RevoluteJointPrimitive 7 | { 8 | Body* bodyA = nullptr; 9 | Body* bodyB = nullptr; 10 | Vector2 localPointA; 11 | Vector2 localPointB; 12 | 13 | real damping = 0.0f; 14 | real stiffness = 0.0f; 15 | real frequency = 8.0f; 16 | real maxForce = 5000.0f; 17 | real dampingRatio = 0.2f; 18 | real gamma = 0.0f; 19 | Vector2 bias; 20 | Matrix2x2 effectiveMass; 21 | Vector2 accumulatedImpulse; 22 | }; 23 | class PHYSICS2D_API RevoluteJoint : public Joint 24 | { 25 | public: 26 | RevoluteJoint() 27 | { 28 | m_type = JointType::Revolute; 29 | } 30 | RevoluteJoint(const RevoluteJointPrimitive& primitive) 31 | { 32 | m_type = JointType::Revolute; 33 | m_primitive = primitive; 34 | } 35 | void set(const RevoluteJointPrimitive& primitive) 36 | { 37 | m_primitive = primitive; 38 | } 39 | void prepare(const real& dt) override 40 | { 41 | if (m_primitive.bodyA == nullptr || m_primitive.bodyB == nullptr) 42 | return; 43 | Body* bodyA = m_primitive.bodyA; 44 | Body* bodyB = m_primitive.bodyB; 45 | 46 | real m_a = bodyA->mass(); 47 | real im_a = bodyA->inverseMass(); 48 | real ii_a = bodyA->inverseInertia(); 49 | 50 | real m_b = bodyB->mass(); 51 | real im_b = bodyB->inverseMass(); 52 | real ii_b = bodyB->inverseInertia(); 53 | 54 | 55 | if (m_primitive.frequency > 0.0) 56 | { 57 | real nf = naturalFrequency(m_primitive.frequency); 58 | m_primitive.stiffness = springStiffness(m_a + m_b, nf); 59 | m_primitive.damping = springDampingCoefficient(m_a + m_b, nf, m_primitive.dampingRatio); 60 | } 61 | else 62 | { 63 | m_primitive.stiffness = 0.0; 64 | m_primitive.damping = 0.0; 65 | } 66 | m_primitive.gamma = constraintImpulseMixing(dt, m_primitive.stiffness, m_primitive.damping); 67 | real erp = errorReductionParameter(dt, m_primitive.stiffness, m_primitive.damping); 68 | 69 | Vector2 pa = bodyA->toWorldPoint(m_primitive.localPointA); 70 | Vector2 ra = pa - bodyA->position(); 71 | Vector2 pb = bodyB->toWorldPoint(m_primitive.localPointB); 72 | Vector2 rb = pb - bodyB->position(); 73 | 74 | m_primitive.bias = (pa - pb) * erp; 75 | Matrix2x2 k; 76 | k.e11() = im_a + ra.y * ra.y * ii_a + im_b + rb.y * rb.y * ii_b; 77 | k.e12() = -ra.x * ra.y * ii_a - rb.x * rb.y * ii_b; 78 | k.e21() = k.e12(); 79 | k.e22() = im_a + ra.x * ra.x * ii_a + im_b + rb.x * rb.x * ii_b; 80 | 81 | k.e11() += m_primitive.gamma; 82 | k.e22() += m_primitive.gamma; 83 | 84 | m_primitive.effectiveMass = k.invert(); 85 | m_primitive.bodyA->applyImpulse(m_primitive.accumulatedImpulse, ra); 86 | m_primitive.bodyB->applyImpulse(-m_primitive.accumulatedImpulse, rb); 87 | 88 | } 89 | void solveVelocity(const real& dt) override 90 | { 91 | if (m_primitive.bodyA == nullptr || m_primitive.bodyB == nullptr) 92 | return; 93 | 94 | Vector2 ra = m_primitive.bodyA->toWorldPoint(m_primitive.localPointA) - m_primitive.bodyA->position(); 95 | Vector2 va = m_primitive.bodyA->velocity() + Vector2::crossProduct(m_primitive.bodyA->angularVelocity(), ra); 96 | Vector2 rb = m_primitive.bodyB->toWorldPoint(m_primitive.localPointB) - m_primitive.bodyB->position(); 97 | Vector2 vb = m_primitive.bodyB->velocity() + Vector2::crossProduct(m_primitive.bodyB->angularVelocity(), rb); 98 | 99 | Vector2 jvb = va - vb; 100 | jvb += m_primitive.bias; 101 | jvb += m_primitive.accumulatedImpulse * m_primitive.gamma; 102 | jvb.negate(); 103 | Vector2 J = m_primitive.effectiveMass.multiply(jvb); 104 | Vector2 oldImpulse = m_primitive.accumulatedImpulse; 105 | m_primitive.accumulatedImpulse += J; 106 | real maxImpulse = dt * m_primitive.maxForce; 107 | if (m_primitive.accumulatedImpulse.lengthSquare() > maxImpulse * maxImpulse) 108 | { 109 | m_primitive.accumulatedImpulse.normalize(); 110 | m_primitive.accumulatedImpulse *= maxImpulse; 111 | } 112 | J = m_primitive.accumulatedImpulse - oldImpulse; 113 | m_primitive.bodyA->applyImpulse(J, ra); 114 | m_primitive.bodyB->applyImpulse(-J, rb); 115 | 116 | } 117 | void solvePosition(const real& dt) override 118 | { 119 | if (m_primitive.bodyA == nullptr || m_primitive.bodyB == nullptr) 120 | return; 121 | //Body* bodyA = m_primitive.bodyA; 122 | //Body* bodyB = m_primitive.bodyB; 123 | //Vector2 pa = bodyA->toWorldPoint(m_primitive.localPointA); 124 | //Vector2 ra = pa - bodyA->position(); 125 | //Vector2 pb = bodyB->toWorldPoint(m_primitive.localPointB); 126 | //Vector2 rb = pb - bodyB->position(); 127 | 128 | //Vector2 bias = (pa - pb) * 0.001f; 129 | //Vector2 impulse = m_primitive.effectiveMass.multiply(bias); 130 | 131 | //if (bodyA->type() != Body::BodyType::Static && !bodyA->sleep()) 132 | //{ 133 | // bodyA->position() += bodyA->inverseMass() * impulse; 134 | // bodyA->rotation() += bodyA->inverseInertia() * ra.cross(impulse); 135 | //} 136 | //if (bodyB->type() != Body::BodyType::Static && !bodyB->sleep()) 137 | //{ 138 | // bodyB->position() -= bodyB->inverseMass() * impulse; 139 | // bodyB->rotation() -= bodyB->inverseInertia() * rb.cross(impulse); 140 | //} 141 | } 142 | RevoluteJointPrimitive& primitive() 143 | { 144 | return m_primitive; 145 | } 146 | private: 147 | RevoluteJointPrimitive m_primitive; 148 | }; 149 | } 150 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_rotation_joint.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_DYNAMICS_JOINT_ANGLE_H 2 | #define PHYSICS2D_DYNAMICS_JOINT_ANGLE_H 3 | #include "physics2d_joint.h" 4 | namespace Physics2D 5 | { 6 | struct PHYSICS2D_API RotationJointPrimitive 7 | { 8 | Body* bodyA; 9 | Body* bodyB; 10 | real referenceRotation = 0; 11 | real effectiveMass = 0; 12 | real bias = 0; 13 | }; 14 | struct PHYSICS2D_API OrientationJointPrimitive 15 | { 16 | Body* bodyA; 17 | Vector2 targetPoint; 18 | real referenceRotation = 0; 19 | real bias = 0; 20 | real effectiveMass = 0; 21 | }; 22 | class PHYSICS2D_API RotationJoint: public Joint 23 | { 24 | public: 25 | RotationJoint() 26 | { 27 | m_type = JointType::Rotation; 28 | } 29 | RotationJoint(const RotationJointPrimitive& prim) : m_primitive(prim) 30 | { 31 | m_type = JointType::Rotation; 32 | } 33 | void set(const RotationJointPrimitive& prim) 34 | { 35 | m_primitive = prim; 36 | } 37 | void prepare(const real& dt) override 38 | { 39 | if (m_primitive.bodyA == nullptr || m_primitive.bodyB == nullptr) 40 | return; 41 | 42 | real ii_a = m_primitive.bodyA->inverseInertia(); 43 | real ii_b = m_primitive.bodyB->inverseInertia(); 44 | real inv_dt = 1.0f / dt; 45 | m_primitive.effectiveMass = 1.0f / (ii_a + ii_b); 46 | real c = m_primitive.bodyA->rotation() - m_primitive.bodyB->rotation() - m_primitive.referenceRotation; 47 | m_primitive.bias = -m_factor * inv_dt * c; 48 | } 49 | void solveVelocity(const real& dt) override 50 | { 51 | real dw = m_primitive.bodyA->angularVelocity() - m_primitive.bodyB->angularVelocity(); 52 | real impulse = m_primitive.effectiveMass * (-dw + m_primitive.bias); 53 | 54 | m_primitive.bodyA->angularVelocity() += m_primitive.bodyA->inverseInertia() * impulse; 55 | m_primitive.bodyB->angularVelocity() -= m_primitive.bodyB->inverseInertia() * impulse; 56 | 57 | } 58 | void solvePosition(const real& dt) override 59 | { 60 | 61 | } 62 | RotationJointPrimitive primitive()const 63 | { 64 | return m_primitive; 65 | } 66 | private: 67 | RotationJointPrimitive m_primitive; 68 | real m_factor = 0.2f; 69 | }; 70 | class PHYSICS2D_API OrientationJoint : public Joint 71 | { 72 | 73 | public: 74 | OrientationJoint() 75 | { 76 | m_type = JointType::Orientation; 77 | } 78 | OrientationJoint(const OrientationJointPrimitive& prim) : m_primitive(prim) 79 | { 80 | m_type = JointType::Orientation; 81 | } 82 | void set(const OrientationJointPrimitive& prim) 83 | { 84 | m_primitive = prim; 85 | } 86 | void prepare(const real& dt) override 87 | { 88 | if (m_primitive.bodyA == nullptr) 89 | return; 90 | 91 | Body* bodyA = m_primitive.bodyA; 92 | Vector2 point = m_primitive.targetPoint - bodyA->position(); 93 | real targetRotation = point.theta(); 94 | 95 | real ii_a = m_primitive.bodyA->inverseInertia(); 96 | real inv_dt = 1.0f / dt; 97 | m_primitive.effectiveMass = 1.0f / ii_a; 98 | real c = targetRotation - m_primitive.bodyA->rotation() - m_primitive.referenceRotation; 99 | if(fuzzyRealEqual(c, 2.0f * Constant::Pi, 0.1f)) 100 | { 101 | c = 0; 102 | bodyA->rotation() = targetRotation; 103 | return; 104 | } 105 | if (fuzzyRealEqual(c, -2.0f * Constant::Pi, 0.1f)) 106 | { 107 | c = 0; 108 | bodyA->rotation() = targetRotation; 109 | return; 110 | } 111 | m_primitive.bias = m_factor * inv_dt * c; 112 | } 113 | void solveVelocity(const real& dt) override 114 | { 115 | real dw = m_primitive.bodyA->angularVelocity(); 116 | real impulse = m_primitive.effectiveMass * (-dw + m_primitive.bias); 117 | 118 | m_primitive.bodyA->angularVelocity() += m_primitive.bodyA->inverseInertia() * impulse; 119 | 120 | } 121 | void solvePosition(const real& dt) override 122 | { 123 | 124 | } 125 | OrientationJointPrimitive primitive()const 126 | { 127 | return m_primitive; 128 | } 129 | private: 130 | OrientationJointPrimitive m_primitive; 131 | real m_factor = 1.0; 132 | }; 133 | } 134 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_sap.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS_BROADPHASE_SAP_H 2 | #define PHYSICS_BROADPHASE_SAP_H 3 | #include "physics2d_body.h" 4 | 5 | namespace Physics2D 6 | { 7 | class PHYSICS2D_API SweepAndPrune 8 | { 9 | public: 10 | 11 | static Container::Vector> generate(const Container::Vector& bodyList); 12 | static Container::Vector query(const Container::Vector& bodyList, const AABB& region); 13 | }; 14 | } 15 | 16 | #endif // !PHYSICS_BROADPHASE_GRID_H 17 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_shape.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_SHAPE_H 2 | #define PHYSICS2D_SHAPE_H 3 | #include "physics2d_linear.h" 4 | #include "physics2d_common.h" 5 | #include "physics2d_algorithm_2d.h" 6 | 7 | namespace Physics2D 8 | { 9 | class PHYSICS2D_API Shape 10 | { 11 | public: 12 | enum class Type 13 | { 14 | Polygon, 15 | Edge, 16 | Capsule, 17 | Circle, 18 | Ellipse 19 | }; 20 | 21 | Type type() const 22 | { 23 | return m_type; 24 | } 25 | 26 | virtual void scale(const real& factor) = 0; 27 | 28 | virtual ~Shape() 29 | { 30 | }; 31 | virtual bool contains(const Vector2& point, const real& epsilon = Constant::GeometryEpsilon) = 0; 32 | virtual Vector2 center() const = 0; 33 | 34 | protected: 35 | Type m_type; 36 | }; 37 | 38 | struct PHYSICS2D_API Transform 39 | { 40 | //refer https://docs.unity3d.com/ScriptReference/Transform.html 41 | Vector2 position; 42 | real rotation = 0; 43 | real scale = 1.0f; 44 | 45 | Vector2 translatePoint(const Vector2& source) const 46 | { 47 | return Matrix2x2(rotation).multiply(source) * scale + position; 48 | } 49 | 50 | Vector2 inverseTranslatePoint(const Vector2& source) const 51 | { 52 | return Matrix2x2(-rotation).multiply(source - position) / scale; 53 | } 54 | 55 | Vector2 inverseRotatePoint(const Vector2& point) const 56 | { 57 | return Matrix2x2(-rotation).multiply(point); 58 | } 59 | }; 60 | 61 | /** 62 | * \brief Basic Shape Description Primitive. Including shape and transform. 63 | */ 64 | struct PHYSICS2D_API ShapePrimitive 65 | { 66 | ShapePrimitive() = default; 67 | Shape* shape = nullptr; 68 | Transform transform; 69 | 70 | bool contains(const Vector2& point, const real& epsilon = Constant::GeometryEpsilon) const 71 | { 72 | if (shape == nullptr) 73 | return false; 74 | return shape->contains(transform.inverseTranslatePoint(point), epsilon); 75 | } 76 | }; 77 | } 78 | #endif 79 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_simplex.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_SIMPLEX_H 2 | #define PHYSICS2D_SIMPLEX_H 3 | 4 | #include "physics2d_algorithm_2d.h" 5 | #include 6 | 7 | namespace Physics2D 8 | { 9 | struct PHYSICS2D_API SimplexVertex 10 | { 11 | SimplexVertex() = default; 12 | 13 | SimplexVertex(const Vector2& point_a, const Vector2& point_b, const Index& index_a = UINT32_MAX, 14 | const Index& index_b = UINT32_MAX) 15 | { 16 | point[0] = point_a; 17 | point[1] = point_b; 18 | result = point[0] - point[1]; 19 | index[0] = index_a; 20 | index[1] = index_b; 21 | } 22 | 23 | bool operator ==(const SimplexVertex& rhs) const 24 | { 25 | return point[0] == rhs.point[0] && point[1] == rhs.point[1]; 26 | } 27 | 28 | bool operator !=(const SimplexVertex& rhs) const 29 | { 30 | return !(point[0] == rhs.point[0] && point[1] == rhs.point[1]); 31 | } 32 | 33 | bool isEmpty() const 34 | { 35 | return point[0].isOrigin() && point[1].isOrigin() && result.isOrigin() && index[0] == UINT32_MAX && index[1] 36 | == UINT32_MAX; 37 | } 38 | 39 | bool isIndexAValid() const 40 | { 41 | return index[0] != UINT32_MAX; 42 | } 43 | 44 | bool isIndexBValid() const 45 | { 46 | return index[1] != UINT32_MAX; 47 | } 48 | 49 | void clear() 50 | { 51 | point[0].clear(); 52 | point[1].clear(); 53 | result.clear(); 54 | index[0] = UINT32_MAX; 55 | index[1] = UINT32_MAX; 56 | } 57 | 58 | //point[0] : pointA 59 | //point[1] : pointB 60 | Vector2 point[2]; 61 | Vector2 result; 62 | //for polygon, the index of the vertex 63 | //index[0] : indexA 64 | //index[1] : indexB 65 | Index index[2]; 66 | }; 67 | 68 | /** 69 | * \brief Simplex Vertex Array for gjk/epa test 70 | * bottleneck: frequently insert operation 71 | */ 72 | struct PHYSICS2D_API SimplexVertexArray 73 | { 74 | Container::Vector vertices; 75 | bool isContainOrigin = false; 76 | bool containOrigin(bool strict = false); 77 | static bool containOrigin(const SimplexVertexArray& simplex, bool strict = false); 78 | 79 | void insert(const size_t& pos, const SimplexVertex& vertex); 80 | bool contains(const SimplexVertex& vertex); 81 | bool fuzzyContains(const SimplexVertex& vertex, const real& epsilon = 0.0001); 82 | 83 | Vector2 lastVertex() const; 84 | }; 85 | 86 | /** 87 | * \brief Simplex structure 88 | */ 89 | struct PHYSICS2D_API Simplex 90 | { 91 | Simplex() = default; 92 | std::array vertices; 93 | size_t count = 0; 94 | bool isContainOrigin = false; 95 | bool containsOrigin(bool strict = false); 96 | static bool containOrigin(const Simplex& simplex, bool strict = false); 97 | bool contains(const SimplexVertex& vertex, const real& epsilon = Constant::GeometryEpsilon); 98 | void addSimplexVertex(const SimplexVertex& vertex); 99 | void removeByIndex(const Index& index); 100 | void removeEnd(); 101 | void removeAll(); 102 | }; 103 | } 104 | #endif 105 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_system.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_SYSTEM_H 2 | #define PHYSICS2D_SYSTEM_H 3 | #include "physics2d_body.h" 4 | #include "physics2d_world.h" 5 | #include "physics2d_detector.h" 6 | #include "physics2d_tree.h" 7 | #include "physics2d_ccd.h" 8 | #include "physics2d_sap.h" 9 | #include "physics2d_grid.h" 10 | 11 | namespace Physics2D 12 | { 13 | class PHYSICS2D_API PhysicsSystem 14 | { 15 | public: 16 | void step(const real& dt); 17 | PhysicsWorld& world(); 18 | ContactMaintainer& maintainer(); 19 | Tree& tree(); 20 | UniformGrid& grid(); 21 | int& positionIteration(); 22 | int& velocityIteration(); 23 | bool& sliceDeltaTime(); 24 | bool& solveJointVelocity(); 25 | bool& solveJointPosition(); 26 | bool& solveContactVelocity(); 27 | bool& solveContactPosition(); 28 | 29 | private: 30 | void updateTree(); 31 | void updateGrid(); 32 | void solve(const real& dt); 33 | bool solveCCD(const real& dt); 34 | int m_positionIteration = 3; 35 | int m_velocityIteration = 8; 36 | bool m_sliceDeltaTime = false; 37 | bool m_solveJointVelocity = true; 38 | bool m_solveJointPosition = true; 39 | bool m_solveContactVelocity = true; 40 | bool m_solveContactPosition = true; 41 | PhysicsWorld m_world; 42 | ContactMaintainer m_maintainer; 43 | Tree m_tree; 44 | UniformGrid m_grid; 45 | }; 46 | } 47 | #endif 48 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_tree.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_BROADPHASE_DBVT_H 2 | #define PHYSICS2D_BROADPHASE_DBVT_H 3 | 4 | #include "physics2d_aabb.h" 5 | 6 | namespace Physics2D 7 | { 8 | /// 9 | /// Dynamic Bounding Volume Tree 10 | /// This is implemented by dynamic array-arranged. 11 | /// 12 | class PHYSICS2D_API Tree 13 | { 14 | public: 15 | struct PHYSICS2D_API Node 16 | { 17 | Body* body = nullptr; 18 | AABB aabb; 19 | int parentIndex = -1; 20 | int leftIndex = -1; 21 | int rightIndex = -1; 22 | bool isLeaf()const; 23 | bool isBranch()const; 24 | bool isRoot()const; 25 | bool isEmpty()const; 26 | void clear(); 27 | 28 | }; 29 | Tree(); 30 | Container::Vector query(Body* body); 31 | Container::Vector query(const AABB& aabb); 32 | Container::Vector raycast(const Vector2& point, const Vector2& direction); 33 | Container::Vector> generate(); 34 | void insert(Body* body); 35 | void remove(Body* body); 36 | void clearAll(); 37 | void update(Body* body); 38 | const Container::Vector& tree(); 39 | int rootIndex()const; 40 | private: 41 | void queryNodes(int nodeIndex, const AABB& aabb, Container::Vector& result); 42 | void traverseLowestCost(int nodeIndex, int boxIndex, real& cost, int& finalIndex); 43 | void raycast(Container::Vector& result, int nodeIndex, const Vector2& p, const Vector2& d); 44 | void generate(int nodeIndex, Container::Vector>& pairs); 45 | void generate(int leftIndex, int rightIndex, Container::Vector>& pairs); 46 | void extract(int targetIndex); 47 | int merge(int nodeIndex, int leafIndex); 48 | void ll(int nodeIndex); 49 | void rr(int nodeIndex); 50 | void balance(int targetIndex); 51 | void separate(int sourceIndex, int parentIndex); 52 | void join(int nodeIndex, int boxIndex); 53 | void remove(int targetIndex); 54 | void elevate(int targetIndex); 55 | void upgrade(int nodeIndex); 56 | int calculateLowestCostNode(int nodeIndex); 57 | real totalCost(int nodeIndex, int leafIndex); 58 | real deltaCost(int nodeIndex, int boxIndex); 59 | size_t allocateNode(); 60 | int height(int targetIndex); 61 | 62 | real m_fatExpansionFactor = 0.5f; 63 | int m_rootIndex = -1; 64 | Container::Vector m_tree; 65 | Container::Vector m_emptyList; 66 | Container::Map m_bodyTable; 67 | }; 68 | 69 | 70 | } 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_vector2.h: -------------------------------------------------------------------------------- 1 | #ifndef MATH_LINEAR_VECTOR2_H 2 | #define MATH_LINEAR_VECTOR2_H 3 | #include "physics2d_common.h" 4 | 5 | namespace Physics2D 6 | { 7 | struct PHYSICS2D_API Vector2 8 | { 9 | Vector2(const real& _x = 0.0, const real& _y = 0.0); 10 | Vector2(const Vector2& copy); 11 | Vector2& operator=(const Vector2& copy); 12 | Vector2(Vector2&& other) = default; 13 | 14 | Vector2 operator+(const Vector2& rhs) const; 15 | Vector2 operator-(const Vector2& rhs) const; 16 | Vector2 operator-() const; 17 | Vector2 operator*(const int& factor) const; 18 | Vector2 operator*(const real& factor) const; 19 | Vector2 operator/(const real& factor) const; 20 | Vector2 operator/(const int& factor) const; 21 | 22 | Vector2& operator+=(const Vector2& rhs); 23 | Vector2& operator-=(const Vector2& rhs); 24 | Vector2& operator*=(const real& factor); 25 | Vector2& operator*=(const int& factor); 26 | Vector2& operator/=(const real& factor); 27 | Vector2& operator/=(const int& factor); 28 | 29 | bool operator==(const Vector2& rhs) const; 30 | bool operator!=(const Vector2& rhs) const; 31 | bool equal(const Vector2& rhs) const; 32 | bool fuzzyEqual(const Vector2& rhs, const real& epsilon = Constant::GeometryEpsilon) const; 33 | bool isOrigin(const real& epsilon = Constant::GeometryEpsilon) const; 34 | bool isSameQuadrant(const Vector2& rhs) const; 35 | 36 | real lengthSquare() const; 37 | real length() const; 38 | real theta() const; 39 | Vector2 normal() const; 40 | Vector2 negative() const; 41 | 42 | 43 | Vector2& set(const real& _x, const real& _y); 44 | Vector2& set(const Vector2& copy); 45 | Vector2& clear(); 46 | Vector2& negate(); 47 | Vector2& swap(Vector2& other) noexcept; 48 | 49 | Vector2& normalize(); 50 | Vector2 perpendicular() const; 51 | 52 | real dot(const Vector2& rhs) const; 53 | real cross(const Vector2& rhs) const; 54 | 55 | Vector2& matchSign(const Vector2& rhs); 56 | 57 | static real dotProduct(const Vector2& lhs, const Vector2& rhs); 58 | static real crossProduct(const Vector2& lhs, const Vector2& rhs); 59 | static real crossProduct(const real& x1, const real& y1, const real& x2, const real& y2); 60 | static Vector2 crossProduct(const real& lhs, const Vector2& rhs); 61 | static Vector2 crossProduct(const Vector2& lhs, const real& rhs); 62 | static Vector2 lerp(const Vector2& lhs, const Vector2& rhs, const real& t); 63 | real x; 64 | real y; 65 | }; 66 | } 67 | #endif 68 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_vector3.h: -------------------------------------------------------------------------------- 1 | #ifndef MATH_LINEAR_VECTOR3_H 2 | #define MATH_LINEAR_VECTOR3_H 3 | #include "physics2d_common.h" 4 | namespace Physics2D 5 | { 6 | struct PHYSICS2D_API Vector3 7 | { 8 | Vector3(const real& x = 0.0, const real& y = 0.0, const real& z = 0.0); 9 | Vector3(const Vector3& copy); 10 | Vector3& operator=(const Vector3& copy); 11 | Vector3(Vector3&& other) = default; 12 | 13 | Vector3 operator+(const Vector3& rhs)const; 14 | Vector3 operator-(const Vector3& other)const; 15 | Vector3 operator-()const; 16 | Vector3 operator*(const real& factor)const; 17 | Vector3 operator*(const int& factor)const; 18 | Vector3 operator/(const real& factor)const; 19 | Vector3 operator/(const int& factor)const; 20 | 21 | Vector3& operator+=(const Vector3& rhs); 22 | Vector3& operator-=(const Vector3& rhs); 23 | Vector3& operator*=(const real& factor); 24 | Vector3& operator*=(const int& factor); 25 | Vector3& operator/=(const real& factor); 26 | Vector3& operator/=(const int& factor); 27 | 28 | Vector3& set(const real& x, const real& y, const real& z); 29 | Vector3& set(const Vector3& other); 30 | Vector3& clear(); 31 | Vector3& negate(); 32 | Vector3& normalize(); 33 | 34 | real lengthSquare()const; 35 | real length()const; 36 | 37 | Vector3 normal()const; 38 | Vector3 negative()const; 39 | 40 | bool equal(const Vector3& rhs)const; 41 | bool fuzzyEqual(const Vector3& rhs, const real& epsilon = Constant::GeometryEpsilon)const; 42 | bool isOrigin(const real& epsilon = Constant::GeometryEpsilon)const; 43 | Vector3& swap(Vector3& other); 44 | 45 | real dot(const Vector3& rhs)const; 46 | Vector3& cross(const Vector3& rhs); 47 | 48 | static real dotProduct(const Vector3& lhs, const Vector3& rhs); 49 | static Vector3 crossProduct(const Vector3& lhs, const Vector3& rhs); 50 | 51 | real x; 52 | real y; 53 | real z; 54 | }; 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /Physics2D/include/physics2d_vector4.h: -------------------------------------------------------------------------------- 1 | #ifndef MATH_LINEAR_VECTOR4_H 2 | #define MATH_LINEAR_VECTOR4_H 3 | #include "physics2d_vector3.h" 4 | namespace Physics2D 5 | { 6 | struct PHYSICS2D_API Vector4 7 | { 8 | Vector4(const real& x = 0.0, const real& y = 0.0, const real& z = 0.0, const real& w = 0.0); 9 | Vector4& operator=(const Vector4& copy); 10 | Vector4& operator=(const Vector3& copy); 11 | Vector4(const Vector4& copy) = default; 12 | Vector4(Vector4&& other) = default; 13 | Vector4(const Vector3& copy); 14 | 15 | Vector4 operator+(const Vector4& rhs)const; 16 | Vector4 operator-(const Vector4& other)const; 17 | Vector4 operator-()const; 18 | Vector4 operator*(const real& factor)const; 19 | Vector4 operator/(const real& factor)const; 20 | 21 | Vector4& operator+=(const Vector4& rhs); 22 | Vector4& operator-=(const Vector4& rhs); 23 | Vector4& operator*=(const real& factor); 24 | Vector4& operator/=(const real& factor); 25 | 26 | Vector4& set(const real& x, const real& y, const real& z, const real& w); 27 | Vector4& set(const Vector4& other); 28 | Vector4& set(const Vector3& other); 29 | Vector4& clear(); 30 | Vector4& negate(); 31 | Vector4& normalize(); 32 | 33 | real lengthSquare()const; 34 | real length()const; 35 | 36 | Vector4 normal()const; 37 | Vector4 negative()const; 38 | 39 | bool equal(const Vector4& rhs)const; 40 | bool fuzzyEqual(const Vector4& rhs, const real& epsilon = Constant::GeometryEpsilon)const; 41 | bool isOrigin(const real& epsilon = Constant::GeometryEpsilon)const; 42 | Vector4& swap(Vector4& other); 43 | 44 | real dot(const Vector4& rhs)const; 45 | Vector4& cross(const Vector4& rhs); 46 | 47 | static real dotProduct(const Vector4& lhs, const Vector4& rhs); 48 | static Vector4 crossProduct(const Vector4& lhs, const Vector4& rhs); 49 | 50 | 51 | real x; 52 | real y; 53 | real z; 54 | real w; 55 | }; 56 | 57 | 58 | } 59 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_weld_joint.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_DYNAMICS_JOINT_WELD_H 2 | #define PHYSICS2D_DYNAMICS_JOINT_WELD_H 3 | #include "physics2d_joint.h" 4 | namespace Physics2D 5 | { 6 | struct PHYSICS2D_API WeldJointPrimitive 7 | { 8 | Body* bodyA = nullptr; 9 | Body* bodyB = nullptr; 10 | Vector2 localPointA; 11 | Vector2 localPointB; 12 | 13 | real damping = 0.0f; 14 | real stiffness = 0.0f; 15 | real frequency = 8.0f; 16 | real maxForce = 5000.0f; 17 | real dampingRatio = 0.2f; 18 | real gamma = 0.0f; 19 | Vector2 bias; 20 | Matrix2x2 effectiveMass; 21 | Vector2 accumulatedImpulse; 22 | }; 23 | //weld joint comes from revolute and rotation joint 24 | class PHYSICS2D_API WeldJoint : public Joint 25 | { 26 | public: 27 | WeldJoint() 28 | { 29 | m_type = JointType::Weld; 30 | } 31 | 32 | WeldJoint(const WeldJointPrimitive& primitive) 33 | { 34 | m_type = JointType::Weld; 35 | m_primitive = primitive; 36 | } 37 | void set(const WeldJointPrimitive& primitive) 38 | { 39 | m_primitive = primitive; 40 | } 41 | void prepare(const real& dt) override 42 | { 43 | if (m_primitive.bodyA == nullptr || m_primitive.bodyB == nullptr) 44 | return; 45 | Body* bodyA = m_primitive.bodyA; 46 | Body* bodyB = m_primitive.bodyB; 47 | 48 | real m_a = bodyA->mass(); 49 | real im_a = bodyA->inverseMass(); 50 | real ii_a = bodyA->inverseInertia(); 51 | 52 | real m_b = bodyB->mass(); 53 | real im_b = bodyB->inverseMass(); 54 | real ii_b = bodyB->inverseInertia(); 55 | 56 | 57 | if (m_primitive.frequency > 0.0) 58 | { 59 | real nf = naturalFrequency(m_primitive.frequency); 60 | m_primitive.stiffness = springStiffness(m_a + m_b, nf); 61 | m_primitive.damping = springDampingCoefficient(m_a + m_b, nf, m_primitive.dampingRatio); 62 | } 63 | else 64 | { 65 | m_primitive.stiffness = 0.0; 66 | m_primitive.damping = 0.0; 67 | } 68 | m_primitive.gamma = constraintImpulseMixing(dt, m_primitive.stiffness, m_primitive.damping); 69 | real erp = errorReductionParameter(dt, m_primitive.stiffness, m_primitive.damping); 70 | 71 | Vector2 pa = bodyA->toWorldPoint(m_primitive.localPointA); 72 | Vector2 ra = pa - bodyA->position(); 73 | Vector2 pb = bodyB->toWorldPoint(m_primitive.localPointB); 74 | Vector2 rb = pb - bodyB->position(); 75 | 76 | m_primitive.bias = (pa - pb) * erp; 77 | Matrix2x2 k; 78 | k.e11() = im_a + ra.y * ra.y * ii_a + im_b + rb.y * rb.y * ii_b; 79 | k.e12() = -ra.x * ra.y * ii_a - rb.x * rb.y * ii_b; 80 | k.e21() = k.e12(); 81 | k.e22() = im_a + ra.x * ra.x * ii_a + im_b + rb.x * rb.x * ii_b; 82 | 83 | k.e11() += m_primitive.gamma; 84 | k.e22() += m_primitive.gamma; 85 | 86 | m_primitive.effectiveMass = k.invert(); 87 | m_primitive.bodyA->applyImpulse(m_primitive.accumulatedImpulse, ra); 88 | m_primitive.bodyB->applyImpulse(-m_primitive.accumulatedImpulse, rb); 89 | } 90 | void solveVelocity(const real& dt) override 91 | { 92 | if (m_primitive.bodyA == nullptr || m_primitive.bodyB == nullptr) 93 | return; 94 | 95 | Vector2 ra = m_primitive.bodyA->toWorldPoint(m_primitive.localPointA) - m_primitive.bodyA->position(); 96 | Vector2 va = m_primitive.bodyA->velocity() + Vector2::crossProduct(m_primitive.bodyA->angularVelocity(), ra); 97 | Vector2 rb = m_primitive.bodyB->toWorldPoint(m_primitive.localPointB) - m_primitive.bodyB->position(); 98 | Vector2 vb = m_primitive.bodyB->velocity() + Vector2::crossProduct(m_primitive.bodyB->angularVelocity(), rb); 99 | 100 | Vector2 jvb = va - vb; 101 | jvb += m_primitive.bias; 102 | jvb += m_primitive.accumulatedImpulse * m_primitive.gamma; 103 | jvb.negate(); 104 | Vector2 J = m_primitive.effectiveMass.multiply(jvb); 105 | Vector2 oldImpulse = m_primitive.accumulatedImpulse; 106 | m_primitive.accumulatedImpulse += J; 107 | real maxImpulse = dt * m_primitive.maxForce; 108 | if (m_primitive.accumulatedImpulse.lengthSquare() > maxImpulse * maxImpulse) 109 | { 110 | m_primitive.accumulatedImpulse.normalize(); 111 | m_primitive.accumulatedImpulse *= maxImpulse; 112 | } 113 | J = m_primitive.accumulatedImpulse - oldImpulse; 114 | m_primitive.bodyA->applyImpulse(J, ra); 115 | m_primitive.bodyB->applyImpulse(-J, rb); 116 | } 117 | void solvePosition(const real& dt) override 118 | { 119 | if (m_primitive.bodyA == nullptr || m_primitive.bodyB == nullptr) 120 | return; 121 | } 122 | WeldJointPrimitive primitive()const 123 | { 124 | return m_primitive; 125 | } 126 | private: 127 | WeldJointPrimitive m_primitive; 128 | }; 129 | } 130 | #endif -------------------------------------------------------------------------------- /Physics2D/include/physics2d_world.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS2D_WORLD_H 2 | #define PHYSICS2D_WORLD_H 3 | #include "physics2d_common.h" 4 | #include "physics2d_body.h" 5 | #include "physics2d_math.h" 6 | #include "physics2d_integrator.h" 7 | #include "physics2d_joints.h" 8 | #include "physics2d_random.h" 9 | #include "physics2d_contact.h" 10 | #include "physics2d_weld_joint.h" 11 | 12 | namespace Physics2D 13 | { 14 | class PHYSICS2D_API PhysicsWorld 15 | { 16 | public: 17 | PhysicsWorld() : m_gravity(0, -9.8f), m_linearVelocityDamping(0.9f), m_angularVelocityDamping(0.9f), 18 | m_linearVelocityThreshold(0.02f), m_angularVelocityThreshold(0.02f), 19 | m_airFrictionCoefficient(0.7f), m_bias(0.8f) 20 | { 21 | } 22 | 23 | ~PhysicsWorld(); 24 | //disable copy 25 | PhysicsWorld(const PhysicsWorld&) = delete; 26 | PhysicsWorld& operator=(const PhysicsWorld&) = delete; 27 | void prepareVelocityConstraint(const real& dt); 28 | void stepVelocity(const real& dt); 29 | void solveVelocityConstraint(real dt); 30 | void stepPosition(const real& dt); 31 | void solvePositionConstraint(real dt); 32 | 33 | 34 | Vector2 gravity() const; 35 | void setGravity(const Vector2& gravity); 36 | 37 | real linearVelocityDamping() const; 38 | void setLinearVelocityDamping(const real& linearVelocityDamping); 39 | 40 | real angularVelocityDamping() const; 41 | void setAngularVelocityDamping(const real& angularVelocityDamping); 42 | 43 | real linearVelocityThreshold() const; 44 | void setLinearVelocityThreshold(const real& linearVelocityThreshold); 45 | 46 | real angularVelocityThreshold() const; 47 | void setAngularVelocityThreshold(const real& angularVelocityThreshold); 48 | 49 | real airFrictionCoefficient() const; 50 | void setAirFrictionCoefficient(const real& airFrictionCoefficient); 51 | 52 | bool enableGravity() const; 53 | void setEnableGravity(bool enableGravity); 54 | 55 | bool enableDamping() const; 56 | void setEnableDamping(bool enableDamping); 57 | 58 | Body* createBody(); 59 | void removeBody(Body* body); 60 | 61 | void removeJoint(Joint* joint); 62 | 63 | void clearAllBodies(); 64 | void clearAllJoints(); 65 | 66 | RotationJoint* createJoint(const RotationJointPrimitive& primitive); 67 | PointJoint* createJoint(const PointJointPrimitive& primitive); 68 | DistanceJoint* createJoint(const DistanceJointPrimitive& primitive); 69 | PulleyJoint* createJoint(const PulleyJointPrimitive& primitive); 70 | RevoluteJoint* createJoint(const RevoluteJointPrimitive& primitive); 71 | WeldJoint* createJoint(const WeldJointPrimitive& primitive); 72 | OrientationJoint* createJoint(const OrientationJointPrimitive& primitive); 73 | 74 | real bias() const; 75 | void setBias(const real& bias); 76 | 77 | Container::Vector>& bodyList(); 78 | 79 | Container::Vector>& jointList(); 80 | 81 | bool& enableSleep(); 82 | 83 | private: 84 | Vector2 m_gravity; 85 | real m_linearVelocityDamping; 86 | real m_angularVelocityDamping; 87 | real m_linearVelocityThreshold; 88 | real m_angularVelocityThreshold; 89 | real m_airFrictionCoefficient; 90 | 91 | real m_bias; 92 | 93 | bool m_enableGravity = true; 94 | bool m_enableDamping = true; 95 | bool m_enableSleep = false; 96 | Container::Vector> m_bodyList; 97 | Container::Vector> m_jointList; 98 | }; 99 | 100 | class PHYSICS2D_API DiscreteWorld 101 | { 102 | public: 103 | using ObjectID = uint32_t; 104 | ObjectID createBody(const ShapePrimitive& primitive); 105 | ObjectID createJoint(); 106 | 107 | void step(real dt); 108 | void stepPosition(real dt); 109 | void stepVelocity(real dt); 110 | 111 | void removeBody(const ObjectID& id); 112 | void removeJoint(const ObjectID& id); 113 | 114 | void solveVelocity(real dt); 115 | void solvePosition(real dt); 116 | 117 | private: 118 | Container::Vector m_bodyList; 119 | Container::Vector m_jointList; 120 | Container::Vector m_sleepList; 121 | 122 | Vector2 m_gravity; 123 | real m_linearVelocityDamping = 0.9f; 124 | real m_angularVelocityDamping = 0.9f; 125 | real m_linearVelocityThreshold = 0.02f; 126 | real m_angularVelocityThreshold = 0.02f; 127 | real m_airFrictionCoefficient = 0.7f; 128 | }; 129 | } 130 | #endif 131 | -------------------------------------------------------------------------------- /Physics2D/source/collision/physics2d_aabb.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_aabb.h" 2 | 3 | #include "physics2d_body.h" 4 | #include "physics2d_algorithm_2d.h" 5 | #include "physics2d_narrowphase.h" 6 | 7 | namespace Physics2D 8 | { 9 | 10 | bool AABB::isEmpty() const 11 | { 12 | return realEqual(width, 0) && realEqual(height, 0) && position.fuzzyEqual({ 0, 0 }); 13 | } 14 | bool AABB::raycast(const Vector2& start, const Vector2& direction) const 15 | { 16 | return raycast(*this, start, direction); 17 | } 18 | 19 | AABB::AABB(const Vector2& topLeft, const real& boxWidth, const real& boxHeight) 20 | { 21 | this->width = boxWidth; 22 | this->height = boxHeight; 23 | this->position = topLeft + Vector2(boxWidth * 0.5f, -boxHeight*0.5f); 24 | } 25 | 26 | AABB::AABB(const Vector2& topLeft, const Vector2& bottomRight) 27 | { 28 | *this = fromBox(topLeft, bottomRight); 29 | } 30 | 31 | Vector2 AABB::topLeft() const 32 | { 33 | return Vector2{ minimumX() , maximumY() }; 34 | } 35 | 36 | Vector2 AABB::topRight() const 37 | { 38 | return Vector2{ maximumX(), maximumY() }; 39 | } 40 | 41 | Vector2 AABB::bottomLeft() const 42 | { 43 | return Vector2{ minimumX() , minimumY() }; 44 | } 45 | 46 | Vector2 AABB::bottomRight() const 47 | { 48 | return Vector2{ maximumX() , minimumY() }; 49 | } 50 | 51 | real AABB::minimumX() const 52 | { 53 | return -width * 0.5f + position.x; 54 | } 55 | 56 | real AABB::minimumY() const 57 | { 58 | return -height * 0.5f + position.y; 59 | } 60 | 61 | real AABB::maximumX() const 62 | { 63 | return width * 0.5f + position.x; 64 | } 65 | 66 | real AABB::maximumY() const 67 | { 68 | return height * 0.5f + position.y; 69 | } 70 | 71 | bool AABB::collide(const AABB& other) const 72 | { 73 | return collide(*this, other); 74 | } 75 | 76 | void AABB::expand(const real& factor) 77 | { 78 | expand(*this, factor); 79 | } 80 | 81 | void AABB::scale(const real& factor) 82 | { 83 | width *= factor; 84 | height *= factor; 85 | } 86 | 87 | 88 | void AABB::clear() 89 | { 90 | position.clear(); 91 | width = 0.0; 92 | height = 0.0; 93 | } 94 | 95 | AABB& AABB::unite(const AABB& other) 96 | { 97 | *this = unite(*this, other); 98 | return *this; 99 | } 100 | 101 | real AABB::surfaceArea() const 102 | { 103 | return (width + height) * 2.0f; 104 | } 105 | 106 | real AABB::volume() const 107 | { 108 | return width * height; 109 | } 110 | 111 | bool AABB::isSubset(const AABB& other) const 112 | { 113 | return isSubset(other, *this); 114 | } 115 | 116 | bool AABB::operator==(const AABB& other) const 117 | { 118 | return position.fuzzyEqual(other.position) && 119 | realEqual(width, other.width) && realEqual(height, other.height); 120 | } 121 | 122 | AABB AABB::fromShape(const ShapePrimitive& shape, const real& factor) 123 | { 124 | AABB aabb; 125 | switch (shape.shape->type()) 126 | { 127 | case Shape::Type::Polygon: 128 | { 129 | const Polygon* polygon = static_cast(shape.shape); 130 | real max_x = Constant::NegativeMin, max_y = Constant::NegativeMin, min_x = Constant::Max, min_y = Constant::Max; 131 | for (const Vector2& v : polygon->vertices()) 132 | { 133 | const Vector2 vertex = Matrix2x2(shape.transform.rotation).multiply(v); 134 | if (max_x < vertex.x) 135 | max_x = vertex.x; 136 | 137 | if (min_x > vertex.x) 138 | min_x = vertex.x; 139 | 140 | if (max_y < vertex.y) 141 | max_y = vertex.y; 142 | 143 | if (min_y > vertex.y) 144 | min_y = vertex.y; 145 | } 146 | aabb.width = std::fabs(max_x - min_x); 147 | aabb.height = std::fabs(max_y - min_y); 148 | aabb.position.set((max_x + min_x) * 0.5f, (max_y + min_y) * 0.5f); 149 | break; 150 | } 151 | case Shape::Type::Ellipse: 152 | { 153 | const Ellipse* ellipse = static_cast(shape.shape); 154 | 155 | Vector2 top_dir{ 0, 1 }; 156 | Vector2 left_dir{ -1, 0 }; 157 | Vector2 bottom_dir{ 0, -1 }; 158 | Vector2 right_dir{ 1, 0 }; 159 | 160 | top_dir = Matrix2x2(-shape.transform.rotation).multiply(top_dir); 161 | left_dir = Matrix2x2(-shape.transform.rotation).multiply(left_dir); 162 | bottom_dir = Matrix2x2(-shape.transform.rotation).multiply(bottom_dir); 163 | right_dir = Matrix2x2(-shape.transform.rotation).multiply(right_dir); 164 | 165 | Vector2 top = GeometryAlgorithm2D::calculateEllipseProjectionPoint(ellipse->A(), ellipse->B(), top_dir); 166 | Vector2 left = GeometryAlgorithm2D::calculateEllipseProjectionPoint(ellipse->A(), ellipse->B(), left_dir); 167 | Vector2 bottom = GeometryAlgorithm2D::calculateEllipseProjectionPoint(ellipse->A(), ellipse->B(), bottom_dir); 168 | Vector2 right = GeometryAlgorithm2D::calculateEllipseProjectionPoint(ellipse->A(), ellipse->B(), right_dir); 169 | 170 | top = Matrix2x2(shape.transform.rotation).multiply(top); 171 | left = Matrix2x2(shape.transform.rotation).multiply(left); 172 | bottom = Matrix2x2(shape.transform.rotation).multiply(bottom); 173 | right = Matrix2x2(shape.transform.rotation).multiply(right); 174 | 175 | aabb.height = std::fabs(top.y - bottom.y); 176 | aabb.width = std::fabs(right.x - left.x); 177 | break; 178 | } 179 | case Shape::Type::Circle: 180 | { 181 | const Circle* circle = static_cast(shape.shape); 182 | aabb.width = circle->radius() * 2; 183 | aabb.height = circle->radius() * 2; 184 | break; 185 | } 186 | case Shape::Type::Edge: 187 | { 188 | const Edge* edge = static_cast(shape.shape); 189 | aabb.width = std::fabs(edge->startPoint().x - edge->endPoint().x); 190 | aabb.height = std::fabs(edge->startPoint().y - edge->endPoint().y); 191 | aabb.position.set(edge->startPoint().x + edge->endPoint().x, edge->startPoint().y + edge->endPoint().y); 192 | aabb.position *= 0.5f; 193 | aabb.expand(0.5f); 194 | break; 195 | } 196 | case Shape::Type::Capsule: 197 | { 198 | auto [p1, idx1] = Narrowphase::findFurthestPoint(shape, {1, 0}); 199 | auto [p2, idx2] = Narrowphase::findFurthestPoint(shape, { 0, 1 }); 200 | p1 -= shape.transform.position; 201 | p2 -= shape.transform.position; 202 | aabb.width = p1.x * 2.0f; 203 | aabb.height = p2.y * 2.0f; 204 | break; 205 | } 206 | } 207 | aabb.position += shape.transform.position; 208 | aabb.expand(factor); 209 | return aabb; 210 | } 211 | 212 | AABB AABB::fromBody(Body* body, const real& factor) 213 | { 214 | assert(body != nullptr); 215 | assert(body->shape() != nullptr); 216 | 217 | ShapePrimitive primitive; 218 | primitive.shape = body->shape(); 219 | primitive.transform.rotation = body->rotation(); 220 | primitive.transform.position = body->position(); 221 | return fromShape(primitive, factor); 222 | } 223 | 224 | AABB AABB::fromBox(const Vector2& topLeft, const Vector2& bottomRight) 225 | { 226 | AABB result; 227 | result.width = bottomRight.x - topLeft.x; 228 | result.height = topLeft.y - bottomRight.y; 229 | result.position = (topLeft + bottomRight) * 0.5f; 230 | return result; 231 | } 232 | 233 | bool AABB::collide(const AABB& src, const AABB& target) 234 | { 235 | const Vector2 srcTopLeft = src.topLeft(); 236 | const Vector2 srcBottomRight = src.bottomRight(); 237 | 238 | const Vector2 targetTopLeft = target.topLeft(); 239 | const Vector2 targetBottomRight = target.bottomRight(); 240 | 241 | return !(srcBottomRight.x < targetTopLeft.x || targetBottomRight.x < srcTopLeft.x || srcTopLeft.y < targetBottomRight.y || targetTopLeft.y < srcBottomRight.y); 242 | } 243 | 244 | AABB AABB::unite(const AABB& src, const AABB& target, const real& factor) 245 | { 246 | if (src.isEmpty()) 247 | return target; 248 | 249 | if (target.isEmpty()) 250 | return src; 251 | 252 | 253 | const Vector2 srcTopLeft = src.topLeft(); 254 | const Vector2 srcBottomRight = src.bottomRight(); 255 | 256 | const Vector2 targetTopLeft = target.topLeft(); 257 | const Vector2 targetBottomRight = target.bottomRight(); 258 | 259 | const real low_x = Math::min(srcTopLeft.x, targetTopLeft.x); 260 | const real high_x = Math::max(srcBottomRight.x, targetBottomRight.x); 261 | 262 | const real low_y = Math::min(srcBottomRight.y, targetBottomRight.y); 263 | const real high_y = Math::max(srcTopLeft.y, targetTopLeft.y); 264 | 265 | AABB aabb; 266 | aabb.position.set((low_x + high_x) * 0.5f, (low_y + high_y) * 0.5f); 267 | aabb.width = high_x - low_x; 268 | aabb.height = high_y - low_y; 269 | 270 | aabb.expand(factor); 271 | return aabb; 272 | } 273 | //b is a subset of a 274 | bool AABB::isSubset(const AABB& a, const AABB& b) 275 | { 276 | 277 | const Vector2 aTopLeft = a.topLeft(); 278 | const Vector2 aBottomRight = a.bottomRight(); 279 | 280 | const Vector2 bTopLeft = b.topLeft(); 281 | const Vector2 bBottomRight = b.bottomRight(); 282 | 283 | return aBottomRight.x >= bBottomRight.x && bTopLeft.x >= aTopLeft.x && 284 | aTopLeft.y >= bTopLeft.y && bBottomRight.y >= aBottomRight.y; 285 | } 286 | void AABB::expand(AABB& aabb, const real& factor) 287 | { 288 | aabb.width += factor; 289 | aabb.height += factor; 290 | } 291 | bool AABB::raycast(const AABB& aabb, const Vector2& start, const Vector2& direction) 292 | { 293 | auto result = GeometryAlgorithm2D::raycastAABB(start, direction, aabb.topLeft(), aabb.bottomRight()); 294 | if (!result.has_value()) 295 | return false; 296 | auto [p1, p2] = result.value(); 297 | return GeometryAlgorithm2D::isPointOnAABB(p1, aabb.topLeft(), aabb.bottomRight()) 298 | && GeometryAlgorithm2D::isPointOnAABB(p2, aabb.topLeft(), aabb.bottomRight()); 299 | } 300 | } 301 | -------------------------------------------------------------------------------- /Physics2D/source/collision/physics2d_capsule.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_capsule.h" 2 | namespace Physics2D 3 | { 4 | 5 | Capsule::Capsule(real width, real height) : m_halfWidth(width / 2.0f), m_halfHeight(height / 2.0f) 6 | { 7 | m_type = Type::Capsule; 8 | } 9 | bool Capsule::contains(const Vector2& point, const real& epsilon) 10 | { 11 | real r = 0, h = 0; 12 | Vector2 anchorPoint1, anchorPoint2; 13 | if (m_halfWidth >= m_halfHeight)//Horizontal 14 | { 15 | r = m_halfHeight; 16 | h = m_halfWidth - m_halfHeight; 17 | 18 | anchorPoint1.set(h, 0); 19 | anchorPoint2.set(-h, 0); 20 | if (point.x - anchorPoint1.x <= epsilon && point.x - anchorPoint2.x >= epsilon 21 | && point.y - r <= epsilon && point.y + r >= epsilon) 22 | return true; 23 | } 24 | else//Vertical 25 | { 26 | r = m_halfWidth / 2; 27 | h = m_halfHeight - m_halfWidth; 28 | anchorPoint1.set(0, h); 29 | anchorPoint2.set(0, -h); 30 | 31 | if (point.y - anchorPoint1.y <= epsilon && point.y - anchorPoint2.y >= epsilon 32 | && point.x - r <= epsilon && point.x + r >= epsilon) 33 | return true; 34 | } 35 | if ((anchorPoint1 - point).lengthSquare() - r * r <= epsilon || 36 | (anchorPoint2 - point).lengthSquare() - r * r <= epsilon) 37 | return true; 38 | 39 | return false; 40 | } 41 | 42 | void Capsule::scale(const real& factor) 43 | { 44 | m_halfWidth *= factor; 45 | m_halfHeight *= factor; 46 | } 47 | 48 | Vector2 Capsule::center() const 49 | { 50 | return Vector2(); 51 | } 52 | 53 | void Capsule::set(real width, real height) 54 | { 55 | m_halfWidth = width / 2.0f; 56 | m_halfHeight = height / 2.0f; 57 | } 58 | 59 | void Capsule::setWidth(real width) 60 | { 61 | m_halfWidth = width * 2.0f; 62 | } 63 | 64 | void Capsule::setHeight(real height) 65 | { 66 | m_halfHeight = height * 2.0f; 67 | } 68 | 69 | real Capsule::width()const 70 | { 71 | return 2.0f * m_halfWidth; 72 | } 73 | 74 | real Capsule::height()const 75 | { 76 | return 2.0f * m_halfHeight; 77 | } 78 | 79 | real Capsule::halfWidth() const 80 | { 81 | return m_halfWidth; 82 | } 83 | 84 | real Capsule::halfHeight() const 85 | { 86 | return m_halfHeight; 87 | } 88 | 89 | Vector2 Capsule::topLeft() const 90 | { 91 | Vector2 result; 92 | real r; 93 | if (m_halfWidth >= m_halfHeight)//Horizontal 94 | { 95 | r = m_halfHeight; 96 | result.set(-m_halfWidth + r, r); 97 | } 98 | else//Vertical 99 | { 100 | r = m_halfWidth; 101 | result.set(-r, m_halfHeight - r); 102 | } 103 | return result; 104 | } 105 | Vector2 Capsule::bottomLeft() const 106 | { 107 | return -topRight(); 108 | } 109 | 110 | Vector2 Capsule::topRight() const 111 | { 112 | Vector2 result; 113 | real r; 114 | if (m_halfWidth >= m_halfHeight)//Horizontal 115 | { 116 | r = m_halfHeight; 117 | result.set(m_halfWidth - r, r); 118 | } 119 | else//Vertical 120 | { 121 | r = m_halfWidth; 122 | result.set(r, m_halfHeight - r); 123 | } 124 | return result; 125 | } 126 | 127 | Vector2 Capsule::bottomRight() const 128 | { 129 | return -topLeft(); 130 | } 131 | 132 | Container::Vector Capsule::boxVertices() const 133 | { 134 | Container::Vector vertices; 135 | vertices.reserve(4); 136 | vertices.emplace_back(this->topLeft()); 137 | vertices.emplace_back(this->bottomLeft()); 138 | vertices.emplace_back(this->bottomRight()); 139 | vertices.emplace_back(this->topRight()); 140 | return vertices; 141 | } 142 | } -------------------------------------------------------------------------------- /Physics2D/source/collision/physics2d_ccd.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_ccd.h" 2 | 3 | namespace Physics2D 4 | { 5 | std::tuple CCD::buildTrajectoryAABB(Body* body, const Vector2& target, const real& dt) 6 | { 7 | assert(body != nullptr); 8 | Container::Vector trajectory; 9 | AABB result; 10 | AABB startBox = AABB::fromBody(body); 11 | Body::PhysicsAttribute start = body->physicsAttribute(); 12 | body->stepPosition(dt); 13 | 14 | Body::PhysicsAttribute end = body->physicsAttribute(); 15 | AABB endBox = AABB::fromBody(body); 16 | 17 | if(startBox == endBox && start.velocity.lengthSquare() < Constant::MaxVelocity && std::fabs(start.angularVelocity) < Constant::MaxAngularVelocity) 18 | { 19 | trajectory.emplace_back(AABBShot( startBox, body->physicsAttribute(), 0)); 20 | trajectory.emplace_back(AABBShot( endBox, body->physicsAttribute(), dt )); 21 | return std::make_tuple(trajectory, result); 22 | } 23 | 24 | 25 | result.unite(startBox).unite(endBox); 26 | 27 | body->setPhysicsAttribute(start); 28 | 29 | real slice = 40; 30 | real step = dt / slice; 31 | for(real i = step;i <= dt; i += step) 32 | { 33 | body->stepPosition(step); 34 | AABB aabb = AABB::fromBody(body); 35 | trajectory.emplace_back(AABBShot{aabb, body->physicsAttribute(), i}); 36 | result.unite(aabb); 37 | } 38 | body->setPhysicsAttribute(start); 39 | return std::make_tuple(trajectory, result); 40 | } 41 | std::tuple CCD::buildTrajectoryAABB(Body* body, const real& dt) 42 | { 43 | assert(body != nullptr); 44 | Container::Vector trajectory; 45 | AABB result; 46 | AABB startBox = AABB::fromBody(body); 47 | Body::PhysicsAttribute start = body->physicsAttribute(); 48 | body->stepPosition(dt); 49 | 50 | Body::PhysicsAttribute end = body->physicsAttribute(); 51 | AABB endBox = AABB::fromBody(body); 52 | 53 | if (startBox == endBox && start.velocity.lengthSquare() < Constant::MaxVelocity && std::fabs(start.angularVelocity) < Constant::MaxAngularVelocity) 54 | { 55 | trajectory.emplace_back(AABBShot(startBox, body->physicsAttribute(), 0)); 56 | trajectory.emplace_back(AABBShot(endBox, body->physicsAttribute(), dt)); 57 | return std::make_tuple(trajectory, result); 58 | } 59 | 60 | 61 | result.unite(startBox).unite(endBox); 62 | 63 | body->setPhysicsAttribute(start); 64 | 65 | real slice = 40; 66 | real step = dt / slice; 67 | for (real i = dt / slice; i <= dt;) 68 | { 69 | body->stepPosition(step); 70 | AABB aabb = AABB::fromBody(body); 71 | trajectory.emplace_back(AABBShot{ aabb, body->physicsAttribute(), i }); 72 | result.unite(aabb); 73 | i += step; 74 | } 75 | body->setPhysicsAttribute(start); 76 | return std::make_tuple(trajectory, result); 77 | } 78 | std::optional CCD::findBroadphaseRoot(Body* staticBody, const BroadphaseTrajectory& staticTrajectory, Body* dynamicBody, const BroadphaseTrajectory& dynamicTrajectory, const real& dt) 79 | { 80 | assert(staticBody != nullptr && dynamicBody != nullptr); 81 | 82 | 83 | AABB traj1 = AABB::unite(staticTrajectory[0].aabb, staticTrajectory.back().aabb); 84 | AABB traj2 = AABB::unite(dynamicTrajectory[0].aabb, dynamicTrajectory.back().aabb); 85 | if (!traj1.collide(traj2)) 86 | return std::nullopt; 87 | 88 | IndexSection result; 89 | size_t length = dynamicTrajectory.size(); 90 | bool forwardFound = false; 91 | bool backwardFound = false; 92 | for (size_t i = 0, j = length - 1; i < length - 1; i++, j--) 93 | { 94 | AABB tempForward = AABB::unite(dynamicTrajectory[i].aabb, dynamicTrajectory[i + 1].aabb); 95 | AABB tempBackward = AABB::unite(dynamicTrajectory[j].aabb, dynamicTrajectory[j - 1].aabb); 96 | if (tempForward.collide(traj1) && !forwardFound) 97 | { 98 | result.forward = i; 99 | forwardFound = true; 100 | } 101 | if (tempBackward.collide(traj1) && !backwardFound) 102 | { 103 | result.backward = j; 104 | backwardFound = true; 105 | } 106 | if (forwardFound && backwardFound) 107 | break; 108 | } 109 | return result.forward == -1 ? std::nullopt : 110 | std::optional(result); 111 | 112 | 113 | } 114 | std::optional CCD::findNarrowphaseRoot(Body* staticBody, const BroadphaseTrajectory& staticTrajectory, Body* dynamicBody, const BroadphaseTrajectory& dynamicTrajectory, const IndexSection& index, const real& dt) 115 | { 116 | assert(staticBody != nullptr && dynamicBody != nullptr); 117 | 118 | if (dynamicTrajectory.size() < 2) 119 | return std::nullopt; 120 | 121 | Body::PhysicsAttribute staticOrigin = staticBody->physicsAttribute(); 122 | Body::PhysicsAttribute dynamicOrigin = dynamicBody->physicsAttribute(); 123 | 124 | real startTimestep = 0; 125 | real endTimestep = 0; 126 | 127 | dynamicBody->setPhysicsAttribute(dynamicTrajectory[index.forward].attribute); 128 | startTimestep = dynamicTrajectory[index.forward].time; 129 | endTimestep = dynamicTrajectory[index.backward].time; 130 | 131 | //slice maybe 25~70. It depends on how thin the sticks you set 132 | const real slice = 30.0; 133 | real step = (endTimestep - startTimestep) / slice; 134 | real forwardSteps = 0; 135 | Body::PhysicsAttribute lastAttribute; 136 | //forwarding 137 | bool isFound = false; 138 | while (startTimestep + forwardSteps <= endTimestep) 139 | { 140 | lastAttribute = dynamicBody->physicsAttribute(); 141 | dynamicBody->stepPosition(step); 142 | forwardSteps += step; 143 | if (const bool result = Detector::collide(staticBody, dynamicBody); result) 144 | { 145 | forwardSteps -= step; 146 | dynamicBody->setPhysicsAttribute(lastAttribute); 147 | isFound = true; 148 | break; 149 | } 150 | } 151 | 152 | if (!isFound) 153 | { 154 | staticBody->setPhysicsAttribute(staticOrigin); 155 | dynamicBody->setPhysicsAttribute(dynamicOrigin); 156 | return std::nullopt; 157 | } 158 | 159 | //retracing 160 | step /= 2.0f; 161 | const real epsilon = 0.01f; 162 | unsigned int counter = 0; 163 | while (startTimestep + forwardSteps <= endTimestep) 164 | { 165 | lastAttribute = dynamicBody->physicsAttribute(); 166 | dynamicBody->stepPosition(step); 167 | forwardSteps += step; 168 | if (const auto result = Detector::detect(staticBody, dynamicBody); result.isColliding) 169 | { 170 | if (std::fabs(result.penetration) < epsilon || counter >= Constant::CCDMaxIterations) 171 | { 172 | staticBody->setPhysicsAttribute(staticOrigin); 173 | dynamicBody->setPhysicsAttribute(dynamicOrigin); 174 | return std::optional(startTimestep + forwardSteps); 175 | } 176 | 177 | forwardSteps -= step; 178 | dynamicBody->setPhysicsAttribute(lastAttribute); 179 | step /= 2.0; 180 | } 181 | counter++; 182 | } 183 | 184 | 185 | return std::nullopt; 186 | } 187 | 188 | std::optional> CCD::query(Tree& tree, Body* body, const real& dt) 189 | { 190 | Container::Vector queryList; 191 | assert(body != nullptr); 192 | auto [trajectoryCCD, aabbCCD] = buildTrajectoryAABB(body, dt); 193 | auto potentials = tree.query(aabbCCD); 194 | 195 | for(auto& elem: potentials) 196 | { 197 | //skip detecting itself 198 | if(elem == body) 199 | continue; 200 | 201 | auto [trajectoryElement, aabbElement] = buildTrajectoryAABB(elem, dt); 202 | auto [newCCDTrajectory, newAABB] = buildTrajectoryAABB(body, elem->position(), dt); 203 | auto result = findBroadphaseRoot(elem, trajectoryElement, body, newCCDTrajectory, dt); 204 | if(result.has_value()) 205 | { 206 | auto toi = findNarrowphaseRoot(elem, trajectoryElement, body, newCCDTrajectory, result.value(), dt); 207 | if (toi.has_value()) 208 | queryList.emplace_back(CCDPair(toi.value(), elem)); 209 | } 210 | } 211 | return !queryList.empty() ? std::optional(queryList) 212 | : std::nullopt; 213 | } 214 | 215 | std::optional> CCD::query(UniformGrid& grid, Body* body, const real& dt) 216 | { 217 | Container::Vector queryList; 218 | assert(body != nullptr); 219 | auto [trajectoryCCD, aabbCCD] = buildTrajectoryAABB(body, dt); 220 | auto potentials = grid.query(aabbCCD); 221 | 222 | for (auto& elem : potentials) 223 | { 224 | //skip detecting itself 225 | if (elem == body) 226 | continue; 227 | 228 | auto [trajectoryElement, aabbElement] = buildTrajectoryAABB(elem, dt); 229 | auto [newCCDTrajectory, newAABB] = buildTrajectoryAABB(body, elem->position(), dt); 230 | auto result = findBroadphaseRoot(elem, trajectoryElement, body, newCCDTrajectory, dt); 231 | if (result.has_value()) 232 | { 233 | auto toi = findNarrowphaseRoot(elem, trajectoryElement, body, newCCDTrajectory, result.value(), dt); 234 | if (toi.has_value()) 235 | queryList.emplace_back(CCDPair(toi.value(), elem)); 236 | } 237 | } 238 | return !queryList.empty() ? std::optional(queryList) 239 | : std::nullopt; 240 | } 241 | 242 | std::optional CCD::earliestTOI(const Container::Vector &pairs, const real &epsilon) { 243 | if(pairs.empty()) 244 | return std::nullopt; 245 | 246 | real minToi = Constant::Max; 247 | for (const auto& elem : pairs) 248 | if (elem.toi < minToi) 249 | minToi = elem.toi; 250 | 251 | return minToi; 252 | } 253 | } 254 | -------------------------------------------------------------------------------- /Physics2D/source/collision/physics2d_circle.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_circle.h" 2 | namespace Physics2D 3 | { 4 | Circle::Circle(real radius) : m_radius(radius) 5 | { 6 | m_type = Type::Circle; 7 | } 8 | 9 | real Circle::radius() const 10 | { 11 | return m_radius; 12 | } 13 | 14 | void Circle::setRadius(const real& radius) 15 | { 16 | m_radius = radius; 17 | } 18 | 19 | void Circle::scale(const real& factor) 20 | { 21 | m_radius *= factor; 22 | } 23 | 24 | bool Circle::contains(const Vector2& point, const real& epsilon) 25 | { 26 | return (m_radius * m_radius - point.lengthSquare()) > epsilon; 27 | } 28 | 29 | Vector2 Circle::center()const 30 | { 31 | return Vector2(); 32 | } 33 | } -------------------------------------------------------------------------------- /Physics2D/source/collision/physics2d_collider.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_collider.h" -------------------------------------------------------------------------------- /Physics2D/source/collision/physics2d_detector.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_detector.h" 2 | namespace Physics2D 3 | { 4 | 5 | bool Detector::collide(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB) 6 | { 7 | Simplex simplex = Narrowphase::gjk(shapeA, shapeB); 8 | bool isColliding = simplex.isContainOrigin; 9 | 10 | if (shapeA.transform.position.fuzzyEqual(shapeB.transform.position) && !isColliding) 11 | isColliding = simplex.containsOrigin(true); 12 | 13 | return isColliding; 14 | } 15 | bool Detector::collide(Body* bodyA, Body* bodyB) 16 | { 17 | assert(bodyA != nullptr && bodyB != nullptr); 18 | 19 | ShapePrimitive shapeA, shapeB; 20 | shapeA.shape = bodyA->shape(); 21 | shapeA.transform.rotation = bodyA->rotation(); 22 | shapeA.transform.position = bodyA->position(); 23 | 24 | shapeB.shape = bodyB->shape(); 25 | shapeB.transform.rotation = bodyB->rotation(); 26 | shapeB.transform.position = bodyB->position(); 27 | 28 | return collide(shapeA, shapeB); 29 | } 30 | bool Detector::collide(const ShapePrimitive& shapeA, Body* bodyB) 31 | { 32 | assert(shapeA.shape != nullptr && bodyB != nullptr); 33 | 34 | ShapePrimitive shapeB; 35 | shapeB.shape = bodyB->shape(); 36 | shapeB.transform.rotation = bodyB->rotation(); 37 | shapeB.transform.position = bodyB->position(); 38 | 39 | return collide(shapeA, shapeB); 40 | } 41 | bool Detector::collide(Body* bodyA, const ShapePrimitive& shapeB) 42 | { 43 | assert(shapeB.shape != nullptr && bodyA != nullptr); 44 | 45 | ShapePrimitive shapeA; 46 | shapeA.shape = bodyA->shape(); 47 | shapeA.transform.rotation = bodyA->rotation(); 48 | shapeA.transform.position = bodyA->position(); 49 | 50 | return collide(shapeA, shapeB); 51 | } 52 | Collision Detector::detect(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB) 53 | { 54 | Collision result; 55 | assert(shapeA.shape != nullptr && shapeB.shape != nullptr); 56 | 57 | //not support two edge 58 | if (shapeA.shape->type() == Shape::Type::Edge && shapeB.shape->type() == Shape::Type::Edge) 59 | return result; 60 | 61 | Simplex simplex = Narrowphase::gjk(shapeA, shapeB); 62 | bool isColliding = simplex.isContainOrigin; 63 | 64 | if (shapeA.transform.position.fuzzyEqual(shapeB.transform.position) && !isColliding) 65 | isColliding = simplex.containsOrigin(true); 66 | 67 | if (isColliding) 68 | { 69 | CollisionInfo info = Narrowphase::epa(simplex, shapeA, shapeB); 70 | result.normal = info.normal; 71 | result.isColliding = isColliding; 72 | result.penetration = info.penetration; 73 | if (!realEqual(result.penetration, 0)) 74 | result.contactList = Narrowphase::generateContacts(shapeA, shapeB, info); 75 | else 76 | result.isColliding = false; 77 | } 78 | 79 | return result; 80 | } 81 | Collision Detector::detect(Body* bodyA, const ShapePrimitive& shapeB) 82 | { 83 | Collision result; 84 | 85 | assert(bodyA != nullptr && shapeB.shape != nullptr); 86 | 87 | ShapePrimitive shapeA; 88 | shapeA.shape = bodyA->shape(); 89 | shapeA.transform.rotation = bodyA->rotation(); 90 | shapeA.transform.position = bodyA->position(); 91 | 92 | 93 | result = detect(shapeA, shapeB); 94 | result.bodyA = bodyA; 95 | result.bodyB = nullptr; 96 | 97 | return result; 98 | 99 | } 100 | Collision Detector::detect(const ShapePrimitive& shapeA, Body* bodyB) 101 | { 102 | Collision result; 103 | 104 | assert(shapeA.shape != nullptr && bodyB != nullptr); 105 | 106 | ShapePrimitive shapeB; 107 | shapeB.shape = bodyB->shape(); 108 | shapeB.transform.rotation = bodyB->rotation(); 109 | shapeB.transform.position = bodyB->position(); 110 | 111 | result = detect(shapeA, shapeB); 112 | result.bodyA = nullptr; 113 | result.bodyB = bodyB; 114 | 115 | return result; 116 | } 117 | Collision Detector::detect(Body* bodyA, Body* bodyB) 118 | { 119 | Collision result; 120 | 121 | assert(bodyA != nullptr && bodyB != nullptr); 122 | 123 | if (bodyA == bodyB) 124 | return result; 125 | 126 | if (bodyA->id() > bodyB->id()) 127 | { 128 | Body* temp = bodyA; 129 | bodyA = bodyB; 130 | bodyB = temp; 131 | } 132 | 133 | 134 | ShapePrimitive shapeA, shapeB; 135 | shapeA.shape = bodyA->shape(); 136 | shapeA.transform.rotation = bodyA->rotation(); 137 | shapeA.transform.position = bodyA->position(); 138 | 139 | shapeB.shape = bodyB->shape(); 140 | shapeB.transform.rotation = bodyB->rotation(); 141 | shapeB.transform.position = bodyB->position(); 142 | 143 | result = detect(shapeA, shapeB); 144 | result.bodyA = bodyA; 145 | result.bodyB = bodyB; 146 | 147 | return result; 148 | } 149 | CollisionInfo Detector::distance(const ShapePrimitive& shapeA, const ShapePrimitive& shapeB) 150 | { 151 | assert(shapeA.shape != nullptr && shapeB.shape != nullptr); 152 | return Narrowphase::gjkDistance(shapeA, shapeB); 153 | } 154 | CollisionInfo Detector::distance(Body* bodyA, const ShapePrimitive& shapeB) 155 | { 156 | assert(bodyA != nullptr && shapeB.shape != nullptr); 157 | 158 | ShapePrimitive shapeA; 159 | shapeA.shape = bodyA->shape(); 160 | shapeA.transform.rotation = bodyA->rotation(); 161 | shapeA.transform.position = bodyA->position(); 162 | 163 | return Narrowphase::gjkDistance(shapeA, shapeB); 164 | } 165 | CollisionInfo Detector::distance(const ShapePrimitive& shapeA, Body* bodyB) 166 | { 167 | assert(bodyB != nullptr && shapeA.shape != nullptr); 168 | 169 | ShapePrimitive shapeB; 170 | shapeB.shape = bodyB->shape(); 171 | shapeB.transform.rotation = bodyB->rotation(); 172 | shapeB.transform.position = bodyB->position(); 173 | 174 | return Narrowphase::gjkDistance(shapeA, shapeB); 175 | } 176 | CollisionInfo Detector::distance(Body* bodyA, Body* bodyB) 177 | { 178 | CollisionInfo info; 179 | VertexPair result; 180 | assert(bodyA != nullptr && bodyB != nullptr); 181 | 182 | if (bodyA == bodyB) 183 | return info; 184 | 185 | ShapePrimitive shapeA, shapeB; 186 | shapeA.shape = bodyA->shape(); 187 | shapeA.transform.rotation = bodyA->rotation(); 188 | shapeA.transform.position = bodyA->position(); 189 | 190 | shapeB.shape = bodyB->shape(); 191 | shapeB.transform.rotation = bodyB->rotation(); 192 | shapeB.transform.position = bodyB->position(); 193 | 194 | return Narrowphase::gjkDistance(shapeA, shapeB); 195 | } 196 | } -------------------------------------------------------------------------------- /Physics2D/source/collision/physics2d_edge.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_edge.h" 2 | #include "physics2d_algorithm_2d.h" 3 | namespace Physics2D 4 | { 5 | Edge::Edge() 6 | { 7 | m_type = Type::Edge; 8 | } 9 | 10 | void Edge::set(const Vector2& start, const Vector2& end) 11 | { 12 | m_point[0] = start; 13 | m_point[1] = end; 14 | m_normal = (m_point[1] - m_point[0]).perpendicular().normal().negate(); 15 | } 16 | 17 | void Edge::setStartPoint(const Vector2& start) 18 | { 19 | m_point[0] = start; 20 | } 21 | 22 | void Edge::setEndPoint(const Vector2& end) 23 | { 24 | m_point[1] = end; 25 | } 26 | 27 | Vector2 Edge::startPoint() const 28 | { 29 | return m_point[0]; 30 | } 31 | 32 | Vector2 Edge::endPoint() const 33 | { 34 | return m_point[1]; 35 | } 36 | 37 | void Edge::scale(const real& factor) 38 | { 39 | m_point[0] *= factor; 40 | m_point[1] *= factor; 41 | } 42 | 43 | bool Edge::contains(const Vector2& point, const real& epsilon) 44 | { 45 | return GeometryAlgorithm2D::isPointOnSegment(m_point[0], m_point[1], point); 46 | } 47 | 48 | Vector2 Edge::center()const 49 | { 50 | return (m_point[0] + m_point[1]) / 2.0f; 51 | } 52 | 53 | Vector2 Edge::normal() const 54 | { 55 | return m_normal; 56 | } 57 | 58 | void Edge::setNormal(const Vector2& normal) 59 | { 60 | m_normal = normal; 61 | } 62 | } -------------------------------------------------------------------------------- /Physics2D/source/collision/physics2d_ellipse.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_ellipse.h" 2 | 3 | namespace Physics2D 4 | { 5 | Ellipse::Ellipse(const real& width, const real& height) : m_width(width), m_height(height) 6 | { 7 | m_type = Type::Ellipse; 8 | } 9 | 10 | void Ellipse::set(const Vector2& leftTop, const Vector2& rightBottom) 11 | { 12 | m_width = std::fabs(rightBottom.x - leftTop.x); 13 | m_height = std::fabs(rightBottom.y - leftTop.y); 14 | } 15 | 16 | void Ellipse::set(const real& width, const real& height) 17 | { 18 | m_width = width; 19 | m_height = height; 20 | } 21 | 22 | void Ellipse::setWidth(const real& width) 23 | { 24 | m_width = width; 25 | } 26 | 27 | void Ellipse::setHeight(const real& height) 28 | { 29 | m_height = height; 30 | } 31 | 32 | void Ellipse::scale(const real& factor) 33 | { 34 | m_width *= factor; 35 | m_height *= factor; 36 | } 37 | 38 | bool Ellipse::contains(const Vector2& point, const real& epsilon) 39 | { 40 | const real a = A(); 41 | const real b = B(); 42 | assert(!realEqual(a, 0) && !realEqual(b, 0)); 43 | const real x = m_width > m_height ? point.x : point.y; 44 | const real y = m_width > m_height ? point.y : point.x; 45 | return (x / a) * (x / a) + (y / b) * (y / b) <= 1.0f; 46 | } 47 | 48 | Vector2 Ellipse::center() const 49 | { 50 | return Vector2(); 51 | } 52 | 53 | real Ellipse::width() const 54 | { 55 | return m_width; 56 | } 57 | 58 | real Ellipse::height() const 59 | { 60 | return m_height; 61 | } 62 | 63 | real Ellipse::A() const 64 | { 65 | return m_width > m_height ? m_width / 2.0f : m_height / 2.0f; 66 | } 67 | 68 | real Ellipse::B() const 69 | { 70 | return m_width > m_height ? m_height / 2.0f : m_width / 2.0f; 71 | } 72 | 73 | real Ellipse::C() const 74 | { 75 | const real a = A(); 76 | const real b = B(); 77 | return sqrt(a * a - b * b); 78 | } 79 | } 80 | -------------------------------------------------------------------------------- /Physics2D/source/collision/physics2d_grid.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_grid.h" 2 | 3 | namespace Physics2D 4 | { 5 | UniformGrid::UniformGrid(const real& width, const real& height, const uint32_t rows, const uint32_t columns) 6 | : m_width(width), m_height(height), m_rows(rows), m_columns(columns) 7 | { 8 | updateGrid(); 9 | } 10 | 11 | Container::Vector> UniformGrid::generate() 12 | { 13 | Container::Vector> result; 14 | Container::Map> map; 15 | for (auto&& cell : m_cellsToBodies) 16 | { 17 | if (cell.second.size() > 1) 18 | { 19 | for (auto iterOuter = cell.second.begin(); iterOuter != cell.second.end() - 1; ++iterOuter) 20 | { 21 | for (auto iterInner = iterOuter + 1; iterInner != cell.second.end(); ++iterInner) 22 | { 23 | map[Body::BodyPair::generateBodyPairID(*iterInner, *iterOuter)] = std::make_pair( 24 | *iterInner, *iterOuter); 25 | } 26 | } 27 | } 28 | } 29 | for (auto&& elem : map) 30 | { 31 | if (elem.second.first->aabb().collide(elem.second.second->aabb())) 32 | result.emplace_back(elem.second); 33 | } 34 | return result; 35 | } 36 | 37 | Container::Vector UniformGrid::raycast(const Vector2& p, const Vector2& d) 38 | { 39 | Container::Vector result; 40 | return result; 41 | } 42 | 43 | void UniformGrid::updateAll() 44 | { 45 | for (auto&& elem : m_bodiesToCells) 46 | update(elem.first); 47 | } 48 | 49 | void UniformGrid::update(Body* body) 50 | { 51 | //default option for update 52 | incrementalUpdate(body); 53 | } 54 | 55 | void UniformGrid::insert(Body* body) 56 | { 57 | assert(body != nullptr); 58 | auto iter = m_bodiesToCells.find(body); 59 | if (iter != m_bodiesToCells.end()) 60 | return; 61 | auto cells = queryCells(body->aabb()); 62 | m_bodiesToCells[body] = cells; 63 | for (auto&& elem : cells) 64 | m_cellsToBodies[elem].emplace_back(body); 65 | } 66 | 67 | void UniformGrid::remove(Body* body) 68 | { 69 | assert(body != nullptr); 70 | auto iter = m_bodiesToCells.find(body); 71 | if (iter == m_bodiesToCells.end()) 72 | return; 73 | } 74 | 75 | void UniformGrid::clearAll() 76 | { 77 | m_bodiesToCells.clear(); 78 | m_cellsToBodies.clear(); 79 | } 80 | 81 | Container::Vector UniformGrid::query(const AABB& aabb) 82 | { 83 | Container::Vector result; 84 | auto potentialList = queryCells(aabb); 85 | for (auto& elem : potentialList) 86 | { 87 | auto iter = m_cellsToBodies.find(elem); 88 | if (iter != m_cellsToBodies.end()) 89 | for (auto&& body : iter->second) 90 | result.emplace_back(body); 91 | } 92 | 93 | return result; 94 | } 95 | 96 | int UniformGrid::rows() const 97 | { 98 | return m_rows; 99 | } 100 | 101 | void UniformGrid::setRows(const int& size) 102 | { 103 | m_rows = size; 104 | updateGrid(); 105 | } 106 | 107 | int UniformGrid::columns() const 108 | { 109 | return m_columns; 110 | } 111 | 112 | void UniformGrid::setColumns(const int& size) 113 | { 114 | m_columns = size; 115 | updateGrid(); 116 | } 117 | 118 | real UniformGrid::width() const 119 | { 120 | return m_width; 121 | } 122 | 123 | void UniformGrid::setWidth(const real& size) 124 | { 125 | m_width = size; 126 | updateGrid(); 127 | } 128 | 129 | real UniformGrid::height() const 130 | { 131 | return m_height; 132 | } 133 | 134 | void UniformGrid::setHeight(const real& size) 135 | { 136 | m_height = size; 137 | updateGrid(); 138 | } 139 | 140 | void UniformGrid::updateGrid() 141 | { 142 | changeGridSize(); 143 | updateBodies(); 144 | } 145 | 146 | void UniformGrid::changeGridSize() 147 | { 148 | assert(m_columns != 0 && m_rows != 0); 149 | m_cellWidth = m_width / static_cast(m_columns); 150 | m_cellHeight = m_height / static_cast(m_rows); 151 | } 152 | 153 | void UniformGrid::updateBodies() 154 | { 155 | } 156 | 157 | void UniformGrid::fullUpdate(Body* body) 158 | { 159 | assert(body != nullptr); 160 | auto iter = m_bodiesToCells.find(body); 161 | if (iter == m_bodiesToCells.end()) 162 | return; 163 | auto cells = m_bodiesToCells[body]; 164 | for (auto&& elem : cells) 165 | { 166 | auto& list = m_cellsToBodies[elem]; 167 | auto bodyIter = std::find(list.begin(), list.end(), body); 168 | if (bodyIter != list.end()) 169 | list.erase(bodyIter); 170 | if (list.empty()) 171 | m_cellsToBodies.erase(elem); 172 | } 173 | m_bodiesToCells.erase(iter); 174 | insert(body); 175 | } 176 | 177 | void UniformGrid::incrementalUpdate(Body* body) 178 | { 179 | assert(body != nullptr); 180 | auto iter = m_bodiesToCells.find(body); 181 | if (iter == m_bodiesToCells.end()) 182 | return; 183 | //Incremental update 184 | //cell list must be sorted array 185 | auto oldCellList = m_bodiesToCells[body]; 186 | auto newCellList = queryCells(body->aabb()); 187 | 188 | std::sort(oldCellList.begin(), oldCellList.end(), std::less()); 189 | //New queryCellList has already been sorted. 190 | //std::sort(newCellList.begin(), newCellList.end(), std::less()); 191 | auto changeList = compareCellList(oldCellList, newCellList); 192 | for (auto&& elem : changeList) 193 | { 194 | switch (elem.first) 195 | { 196 | case Operation::Add: 197 | { 198 | m_bodiesToCells[body].emplace_back(elem.second); 199 | m_cellsToBodies[elem.second].emplace_back(body); 200 | break; 201 | } 202 | case Operation::Delete: 203 | { 204 | auto& bodyList = m_cellsToBodies[elem.second]; 205 | std::erase_if(bodyList, 206 | [&body](Body* target) { return body == target; }); 207 | 208 | if (bodyList.empty()) 209 | m_cellsToBodies.erase(elem.second); 210 | 211 | auto& positionList = m_bodiesToCells[body]; 212 | std::erase_if(positionList, 213 | [&elem](const Position& other) { return elem.second == other; }); 214 | 215 | break; 216 | } 217 | } 218 | } 219 | } 220 | 221 | Container::Vector> UniformGrid::compareCellList( 222 | const Container::Vector& oldCellList, const Container::Vector& newCellList) 223 | { 224 | Container::Vector> result; 225 | size_t indexOld = 0, indexNew = 0; 226 | size_t endIndexOld = oldCellList.size(); 227 | size_t endIndexNew = newCellList.size(); 228 | while (true) 229 | { 230 | if (indexOld == endIndexOld && indexNew == endIndexNew) 231 | break; 232 | if (indexNew < endIndexNew && indexOld == endIndexOld) 233 | { 234 | result.emplace_back(std::make_pair(Operation::Add, newCellList[indexNew])); 235 | ++indexNew; 236 | continue; 237 | } 238 | if (indexOld < endIndexOld && indexNew == endIndexNew) 239 | { 240 | result.emplace_back(std::make_pair(Operation::Delete, oldCellList[indexOld])); 241 | ++indexOld; 242 | continue; 243 | } 244 | if (indexOld < endIndexOld && indexNew < endIndexNew) 245 | { 246 | if (oldCellList[indexOld] == newCellList[indexNew]) 247 | { 248 | ++indexOld; 249 | ++indexNew; 250 | continue; 251 | } 252 | if (oldCellList[indexOld] > newCellList[indexNew]) 253 | { 254 | result.emplace_back(std::make_pair(Operation::Add, newCellList[indexNew])); 255 | ++indexNew; 256 | continue; 257 | } 258 | if (oldCellList[indexOld] < newCellList[indexNew]) 259 | { 260 | result.emplace_back(std::make_pair(Operation::Delete, oldCellList[indexOld])); 261 | ++indexOld; 262 | } 263 | } 264 | } 265 | return result; 266 | } 267 | 268 | Container::Vector UniformGrid::queryCells(const AABB& aabb) 269 | { 270 | Container::Vector cells; 271 | //locate x axis 272 | const real halfWidth = m_width * 0.5f; 273 | const real halfHeight = m_height * 0.5f; 274 | const real xRealMin = aabb.minimumX(); 275 | const real xRealMax = aabb.maximumX(); 276 | const real xMin = Math::clamp(xRealMin, -halfWidth, halfWidth - m_cellWidth); 277 | const real xMax = Math::clamp(xRealMax, -halfWidth, halfWidth - m_cellWidth); 278 | const real lowerXIndex = std::floor((xMin + halfWidth) / m_cellWidth); 279 | const real upperXIndex = std::floor((xMax + halfWidth) / m_cellWidth); 280 | //locate y axis 281 | 282 | const real yRealMin = aabb.minimumY(); 283 | const real yRealMax = aabb.maximumY(); 284 | const real yMin = Math::clamp(yRealMin, -halfHeight + m_cellHeight, halfHeight); 285 | const real yMax = Math::clamp(yRealMax, -halfHeight + m_cellHeight, halfHeight); 286 | const real lowerYIndex = std::ceil((yMin + halfHeight) / m_cellHeight); 287 | const real upperYIndex = std::ceil((yMax + halfHeight) / m_cellHeight); 288 | if (lowerXIndex == upperXIndex && xRealMax < -halfWidth || xRealMin > halfWidth) 289 | return cells; 290 | if (lowerYIndex == upperYIndex && yRealMax < -halfHeight || yRealMin > halfHeight) 291 | return cells; 292 | 293 | for (real i = lowerXIndex; i <= upperXIndex; i += 1.0f) 294 | { 295 | for (real j = lowerYIndex; j <= upperYIndex; j += 1.0f) 296 | { 297 | cells.emplace_back(Position{static_cast(i), static_cast(j)}); 298 | } 299 | } 300 | 301 | return cells; 302 | } 303 | 304 | real UniformGrid::cellHeight() const 305 | { 306 | return m_cellHeight; 307 | } 308 | 309 | real UniformGrid::cellWidth() const 310 | { 311 | return m_cellWidth; 312 | } 313 | } 314 | -------------------------------------------------------------------------------- /Physics2D/source/collision/physics2d_polygon.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_polygon.h" 2 | #include "physics2d_algorithm_2d.h" 3 | namespace Physics2D 4 | { 5 | Polygon::Polygon() 6 | { 7 | m_type = Type::Polygon; 8 | m_vertices.reserve(4); 9 | } 10 | 11 | const Container::Vector& Polygon::vertices() const 12 | { 13 | return m_vertices; 14 | } 15 | 16 | void Polygon::append(const std::initializer_list& vertices) 17 | { 18 | for (const Vector2& vertex : vertices) 19 | m_vertices.emplace_back(vertex); 20 | updateVertices(); 21 | } 22 | 23 | void Polygon::append(const Vector2& vertex) 24 | { 25 | m_vertices.emplace_back(vertex); 26 | updateVertices(); 27 | } 28 | 29 | Vector2 Polygon::center()const 30 | { 31 | return GeometryAlgorithm2D::calculateCenter(this->vertices()); 32 | } 33 | 34 | void Polygon::scale(const real& factor) 35 | { 36 | assert(!m_vertices.empty()); 37 | for (Vector2& vertex : m_vertices) 38 | vertex *= factor; 39 | } 40 | 41 | bool Polygon::contains(const Vector2& point, const real& epsilon) 42 | { 43 | for(auto iter = m_vertices.begin(); iter != m_vertices.end(); ++iter) 44 | { 45 | auto next = iter + 1; 46 | if(next == m_vertices.end()) 47 | next = m_vertices.begin(); 48 | auto ref = next + 1; 49 | if(ref == m_vertices.end()) 50 | ref = m_vertices.begin(); 51 | if (!GeometryAlgorithm2D::isPointOnSameSide(*iter, *next, *ref, point)) 52 | return false; 53 | 54 | } 55 | return true; 56 | } 57 | 58 | void Polygon::updateVertices() 59 | { 60 | Vector2 center = this->center(); 61 | for (auto& elem : m_vertices) 62 | elem -= center; 63 | } 64 | } -------------------------------------------------------------------------------- /Physics2D/source/collision/physics2d_rectangle.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_rectangle.h" 2 | namespace Physics2D 3 | { 4 | Rectangle::Rectangle(const real& width, const real& height) 5 | { 6 | m_type = Type::Polygon; 7 | this->set(width, height); 8 | } 9 | 10 | void Rectangle::set(const real& width, const real& height) 11 | { 12 | m_width = width; 13 | m_height = height; 14 | calcVertices(); 15 | } 16 | 17 | real Rectangle::width() const 18 | { 19 | return m_width; 20 | } 21 | 22 | real Rectangle::height() const 23 | { 24 | return m_height; 25 | } 26 | 27 | void Rectangle::setWidth(const real& width) 28 | { 29 | m_width = width; 30 | calcVertices(); 31 | } 32 | 33 | void Rectangle::setHeight(const real& height) 34 | { 35 | m_height = height; 36 | calcVertices(); 37 | } 38 | void Rectangle::scale(const real& factor) 39 | { 40 | m_width *= factor; 41 | m_height *= factor; 42 | calcVertices(); 43 | } 44 | bool Rectangle::contains(const Vector2& point, const real& epsilon) 45 | { 46 | return (point.x < m_width / 2.0 && point.x > -m_width / 2.0) && 47 | point.y < m_height / 2.0 && point.y > -m_height / 2.0; 48 | } 49 | void Rectangle::calcVertices() 50 | { 51 | m_vertices.clear(); 52 | m_vertices.emplace_back(Vector2(-m_width * (0.5f), m_height * (0.5f))); 53 | m_vertices.emplace_back(Vector2(-m_width * (0.5f), -m_height * (0.5f))); 54 | m_vertices.emplace_back(Vector2(m_width * (0.5f), -m_height * (0.5f))); 55 | m_vertices.emplace_back(Vector2(m_width * (0.5f), m_height * (0.5f))); 56 | } 57 | } -------------------------------------------------------------------------------- /Physics2D/source/collision/physics2d_sap.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_sap.h" 2 | 3 | #include "physics2d_body.h" 4 | 5 | namespace Physics2D 6 | { 7 | Container::Vector> SweepAndPrune::generate(const Container::Vector& bodyList) 8 | { 9 | Container::Vector> result; 10 | //sort by x axis 11 | Container::Vector> bodyBoxPairList; 12 | bodyBoxPairList.reserve(bodyList.size()); 13 | 14 | for(auto&& elem: bodyList) 15 | bodyBoxPairList.emplace_back(std::make_pair(elem, elem->aabb())); 16 | 17 | Container::Vector> sortXAxis = bodyBoxPairList; 18 | Container::Vector> sortYAxis = bodyBoxPairList; 19 | std::sort(sortXAxis.begin(), sortXAxis.end(), [](const std::pair& left, const std::pair& right) 20 | { 21 | return left.second.minimumX() < right.second.minimumX(); 22 | }); 23 | std::sort(sortYAxis.begin(), sortYAxis.end(), [](const std::pair& left, const std::pair& right) 24 | { 25 | return left.second.minimumY() < right.second.minimumY(); 26 | }); 27 | 28 | 29 | Container::Vector xPairs; 30 | Container::Vector yPairs; 31 | 32 | for (auto before = sortXAxis.begin(); before != sortXAxis.end(); ++before) 33 | { 34 | for (auto next = std::next(before); next != sortXAxis.end(); ++next) 35 | { 36 | const real minBefore = before->second.minimumX(); 37 | const real maxBefore = before->second.maximumX(); 38 | const real minNext = next->second.minimumX(); 39 | const real maxNext = next->second.maximumX(); 40 | 41 | if (!(maxBefore < minNext || maxNext < minBefore)) 42 | xPairs.emplace_back(Body::BodyPair::generateBodyPair(before->first, next->first)); 43 | else 44 | break; 45 | } 46 | } 47 | 48 | for (auto before = sortYAxis.begin(); before != sortYAxis.end(); ++before) 49 | { 50 | for (auto next = std::next(before); next != sortYAxis.end(); ++next) 51 | { 52 | const real minBefore = before->second.minimumY(); 53 | const real maxBefore = before->second.maximumY(); 54 | const real minNext = next->second.minimumY(); 55 | const real maxNext = next->second.maximumY(); 56 | 57 | if (!(maxBefore < minNext || maxNext < minBefore)) 58 | yPairs.emplace_back(Body::BodyPair::generateBodyPair(before->first, next->first)); 59 | else 60 | break; 61 | } 62 | } 63 | 64 | std::sort(xPairs.begin(), xPairs.end(), [](const Body::BodyPair& left, const Body::BodyPair& right) 65 | { 66 | return left.pairID < right.pairID; 67 | }); 68 | std::sort(yPairs.begin(), yPairs.end(), [](const Body::BodyPair& left, const Body::BodyPair& right) 69 | { 70 | return left.pairID < right.pairID; 71 | }); 72 | 73 | //double pointer check 74 | auto xPair = xPairs.begin(); 75 | auto yPair = yPairs.begin(); 76 | while (xPair != xPairs.end() && yPair != yPairs.end()) 77 | { 78 | if (xPair->pairID == yPair->pairID && (xPair->bodyA->bitmask() & xPair->bodyB->bitmask())) 79 | { 80 | result.emplace_back(std::make_pair(xPair->bodyA, xPair->bodyB)); 81 | xPair = std::next(xPair); 82 | yPair = std::next(yPair); 83 | } 84 | else if(xPair->pairID > yPair->pairID) 85 | yPair = std::next(yPair); 86 | else 87 | xPair = std::next(xPair); 88 | } 89 | 90 | 91 | return result; 92 | } 93 | 94 | Container::Vector SweepAndPrune::query(const Container::Vector& bodyList, const AABB& region) 95 | { 96 | Container::Vector result; 97 | 98 | Container::Vector> bodyBoxPairList; 99 | bodyBoxPairList.reserve(bodyList.size()); 100 | 101 | for (auto&& elem : bodyList) 102 | bodyBoxPairList.emplace_back(std::make_pair(elem, elem->aabb())); 103 | 104 | //brute search 105 | for (auto&& elem : bodyBoxPairList) 106 | if (region.collide(elem.second)) 107 | result.emplace_back(elem.first); 108 | 109 | 110 | 111 | return result; 112 | } 113 | } 114 | -------------------------------------------------------------------------------- /Physics2D/source/collision/physics2d_simplex.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_simplex.h" 2 | namespace Physics2D 3 | { 4 | bool SimplexVertexArray::containOrigin(bool strict) 5 | { 6 | isContainOrigin = containOrigin(*this, strict); 7 | return isContainOrigin; 8 | } 9 | 10 | bool SimplexVertexArray::containOrigin(const SimplexVertexArray& simplex, bool strict) 11 | { 12 | switch (simplex.vertices.size()) 13 | { 14 | case 4: 15 | { 16 | return GeometryAlgorithm2D::triangleContainsOrigin(simplex.vertices[0].result, 17 | simplex.vertices[1].result, simplex.vertices[2].result); 18 | } 19 | case 2: 20 | { 21 | Vector2 oa = simplex.vertices[0].result * -1; 22 | Vector2 ob = simplex.vertices[1].result * -1; 23 | return GeometryAlgorithm2D::isPointOnSegment(oa, ob, { 0, 0 }); 24 | 25 | } 26 | default: 27 | return false; 28 | } 29 | } 30 | 31 | void SimplexVertexArray::insert(const size_t& pos, const SimplexVertex& vertex) 32 | { 33 | vertices.insert(vertices.begin() + pos + 1, vertex); 34 | } 35 | 36 | bool SimplexVertexArray::contains(const SimplexVertex& vertex) 37 | { 38 | return std::find(std::begin(vertices), std::end(vertices), vertex) != std::end(vertices); 39 | } 40 | 41 | bool SimplexVertexArray::fuzzyContains(const SimplexVertex& vertex, const real& epsilon) 42 | { 43 | return std::find_if(std::begin(vertices), std::end(vertices), 44 | [=](const Physics2D::SimplexVertex& element) 45 | { 46 | return (vertex.result - element.result).lengthSquare() < epsilon; 47 | }) 48 | != std::end(vertices); 49 | } 50 | 51 | Vector2 SimplexVertexArray::lastVertex() const 52 | { 53 | if (vertices.size() == 2) 54 | return vertices[vertices.size() - 1].result; 55 | return vertices[vertices.size() - 2].result; 56 | } 57 | 58 | bool Simplex::containsOrigin(bool strict) 59 | { 60 | isContainOrigin = containOrigin(*this, strict); 61 | return isContainOrigin; 62 | } 63 | 64 | bool Simplex::containOrigin(const Simplex& simplex, bool strict) 65 | { 66 | switch (simplex.count) 67 | { 68 | case 1: 69 | return simplex.vertices[0].result.isOrigin() && !simplex.vertices[0].point[0].isOrigin() && !simplex.vertices[0].point[1].isOrigin(); 70 | case 2: 71 | return GeometryAlgorithm2D::isPointOnSegment(simplex.vertices[0].result, simplex.vertices[1].result, { 0, 0 }); 72 | case 3: 73 | return GeometryAlgorithm2D::triangleContainsOrigin(simplex.vertices[0].result, simplex.vertices[1].result, simplex.vertices[2].result); 74 | default: 75 | assert(false, "Simplex count is more than 3"); 76 | return false; 77 | } 78 | } 79 | 80 | bool Simplex::contains(const SimplexVertex& vertex, const real& epsilon) 81 | { 82 | for (SimplexVertex& element : vertices) 83 | if (!element.isEmpty() && (element == vertex || element.result.fuzzyEqual(vertex.result, epsilon))) 84 | return true; 85 | return false; 86 | } 87 | void Simplex::addSimplexVertex(const SimplexVertex& vertex) 88 | { 89 | assert(count < 4); 90 | vertices[count] = vertex; 91 | ++count; 92 | } 93 | 94 | void Simplex::removeByIndex(const Index& index) 95 | { 96 | assert(index < 3); 97 | if(index == 0) 98 | { 99 | vertices[0] = vertices[1]; 100 | vertices[1] = vertices[2]; 101 | } 102 | else if (index == 1) 103 | vertices[1] = vertices[2]; 104 | 105 | vertices[2].clear(); 106 | --count; 107 | } 108 | 109 | void Simplex::removeEnd() 110 | { 111 | vertices[2].clear(); 112 | --count; 113 | } 114 | 115 | void Simplex::removeAll() 116 | { 117 | vertices[0].clear(); 118 | vertices[1].clear(); 119 | vertices[2].clear(); 120 | count = 0; 121 | } 122 | } 123 | -------------------------------------------------------------------------------- /Physics2D/source/dynamics/physics2d_body.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_body.h" 2 | namespace Physics2D { 3 | 4 | Vector2& Body::position() 5 | { 6 | return m_position; 7 | } 8 | 9 | 10 | Vector2& Body::velocity() 11 | { 12 | return m_velocity; 13 | } 14 | 15 | 16 | real& Body::rotation() 17 | { 18 | return m_rotation; 19 | } 20 | 21 | real& Body::angularVelocity() 22 | { 23 | return m_angularVelocity; 24 | } 25 | 26 | Vector2& Body::forces() 27 | { 28 | return m_forces; 29 | } 30 | 31 | 32 | void Body::clearTorque() 33 | { 34 | m_torques = 0; 35 | } 36 | 37 | real& Body::torques() 38 | { 39 | return m_torques; 40 | } 41 | Vector2& Body::lastPosition() 42 | { 43 | return m_lastPosition; 44 | } 45 | real& Body::lastRotation() 46 | { 47 | return m_lastRotation; 48 | } 49 | uint32_t& Body::sleepCountdown() 50 | { 51 | return m_sleepCountdown; 52 | } 53 | Shape* Body::shape() const 54 | { 55 | return m_shape; 56 | } 57 | 58 | void Body::setShape(Shape* shape) 59 | { 60 | m_shape = shape; 61 | calcInertia(); 62 | } 63 | 64 | Body::BodyType Body::type() const 65 | { 66 | return m_type; 67 | } 68 | 69 | void Body::setType(const Body::BodyType &type) 70 | { 71 | m_type = type; 72 | } 73 | 74 | real Body::mass() const 75 | { 76 | return m_mass; 77 | } 78 | 79 | void Body::setMass(const real &mass) 80 | { 81 | m_mass = mass; 82 | 83 | if(realEqual(mass,Constant::Max)) 84 | m_invMass = 0; 85 | else 86 | m_invMass = !realEqual(mass, 0) ? 1.0f / mass : 0; 87 | 88 | calcInertia(); 89 | } 90 | 91 | real Body::inertia() const 92 | { 93 | return m_inertia; 94 | } 95 | 96 | AABB Body::aabb(const real &factor) const 97 | { 98 | ShapePrimitive primitive; 99 | primitive.transform.position = m_position; 100 | primitive.transform.rotation = m_rotation; 101 | primitive.shape = m_shape; 102 | return AABB::fromShape(primitive, factor); 103 | } 104 | 105 | real Body::friction() const 106 | { 107 | return m_friction; 108 | } 109 | 110 | void Body::setFriction(const real &friction) 111 | { 112 | m_friction = friction; 113 | } 114 | 115 | bool Body::sleep() const 116 | { 117 | return m_sleep; 118 | } 119 | 120 | void Body::setSleep(bool sleep) 121 | { 122 | m_sleep = sleep; 123 | } 124 | 125 | real Body::inverseMass() const 126 | { 127 | return m_invMass; 128 | } 129 | 130 | real Body::inverseInertia() const 131 | { 132 | return m_invInertia; 133 | } 134 | 135 | Body::PhysicsAttribute Body::physicsAttribute() const 136 | { 137 | return {m_position, m_velocity, m_rotation, m_angularVelocity}; 138 | } 139 | 140 | void Body::setPhysicsAttribute(const PhysicsAttribute& info) 141 | { 142 | m_position = info.position; 143 | m_rotation = info.rotation; 144 | m_velocity = info.velocity; 145 | m_angularVelocity = info.angularVelocity; 146 | } 147 | 148 | void Body::stepPosition(const real& dt) 149 | { 150 | m_position += m_velocity * dt; 151 | m_rotation += m_angularVelocity * dt; 152 | } 153 | 154 | void Body::applyImpulse(const Vector2& impulse, const Vector2& r) 155 | { 156 | m_velocity += m_invMass * impulse; 157 | m_angularVelocity += m_invInertia * r.cross(impulse); 158 | } 159 | Vector2 Body::toLocalPoint(const Vector2& point)const 160 | { 161 | return Matrix2x2(-m_rotation).multiply(point - m_position); 162 | } 163 | 164 | Vector2 Body::toWorldPoint(const Vector2& point) const 165 | { 166 | return Matrix2x2(m_rotation).multiply(point) + m_position; 167 | } 168 | Vector2 Body::toActualPoint(const Vector2& point) const 169 | { 170 | return Matrix2x2(m_rotation).multiply(point); 171 | } 172 | 173 | uint32_t Body::id() const 174 | { 175 | return m_id; 176 | } 177 | 178 | void Body::setId(const uint32_t& id) 179 | { 180 | m_id = id; 181 | } 182 | 183 | uint32_t Body::bitmask() const 184 | { 185 | return m_bitmask; 186 | } 187 | 188 | void Body::setBitmask(const uint32_t& bitmask) 189 | { 190 | m_bitmask = bitmask; 191 | } 192 | 193 | real Body::restitution() const 194 | { 195 | return m_restitution; 196 | } 197 | 198 | void Body::setRestitution(const real& restitution) 199 | { 200 | m_restitution = restitution; 201 | } 202 | 203 | real Body::kineticEnergy() const 204 | { 205 | const real energyByPos = 0.5f * m_mass * (m_position - m_lastPosition).lengthSquare() + 0.5f * m_inertia * std::pow(m_rotation - m_lastRotation, 2); 206 | const real energyByVel = 0.5f * m_mass * m_velocity.lengthSquare() + 0.5f * m_inertia * m_angularVelocity * m_angularVelocity; 207 | const real mixEnergy = 0.5f * (energyByPos + energyByVel); 208 | return mixEnergy; 209 | } 210 | 211 | void Body::calcInertia() 212 | { 213 | switch (m_shape->type()) { 214 | case Shape::Type::Circle: 215 | { 216 | const Circle* circle = static_cast(m_shape); 217 | 218 | m_inertia = m_mass * circle->radius() * circle->radius() / 2; 219 | break; 220 | } 221 | case Shape::Type::Polygon: 222 | { 223 | const Polygon* polygon = static_cast(m_shape); 224 | 225 | const Vector2 center = polygon->center(); 226 | real sum1 = 0.0; 227 | real sum2 = 0.0; 228 | for (auto iter = polygon->vertices().begin(); iter != polygon->vertices().end(); ++iter) 229 | { 230 | auto next = iter + 1; 231 | if (next == polygon->vertices().end()) 232 | next = polygon->vertices().begin(); 233 | 234 | Vector2 n1 = *iter - center; 235 | Vector2 n2 = *next - center; 236 | real cross = std::fabs(n1.cross(n2)); 237 | real dot = n2.dot(n2) + n2.dot(n1) + n1.dot(n1); 238 | sum1 += cross * dot; 239 | sum2 += cross; 240 | } 241 | 242 | m_inertia = m_mass * (1.0f / 6.0f) * sum1 / sum2; 243 | break; 244 | } 245 | case Shape::Type::Ellipse: 246 | { 247 | const Ellipse* ellipse = static_cast(m_shape); 248 | 249 | const real a = ellipse->A(); 250 | const real b = ellipse->B(); 251 | m_inertia = m_mass * (a * a + b * b) * (1.0f / 5.0f); 252 | 253 | break; 254 | } 255 | case Shape::Type::Capsule: 256 | { 257 | const Capsule* capsule = static_cast(m_shape); 258 | real r = 0, h = 0, massS = 0, inertiaS = 0, massC = 0, inertiaC = 0, volume = 0; 259 | 260 | if (capsule->width() >= capsule->height())//Horizontal 261 | { 262 | r = capsule->height() / 2.0f; 263 | h = capsule->width() - capsule->height(); 264 | } 265 | else//Vertical 266 | { 267 | r = capsule->width() / 2.0f; 268 | h = capsule->height() - capsule->width(); 269 | } 270 | 271 | volume = Constant::Pi * r * r + h * 2 * r; 272 | real rho = m_mass / volume; 273 | massS = rho * Constant::Pi * r * r; 274 | massC = rho * h * 2.0f * r; 275 | inertiaC = (1.0f / 12.0f) * massC * (h * h + (2.0f * r) * (2.0f * r)); 276 | inertiaS = massS * r * r * 0.5f; 277 | m_inertia = inertiaC + inertiaS + massS * (3.0f * r + 2.0f * h) * h / 8.0f; 278 | break; 279 | } 280 | default: 281 | break; 282 | } 283 | if (realEqual(m_mass, Constant::Max)) 284 | m_invInertia = 0; 285 | else 286 | m_invInertia = !realEqual(m_inertia, 0) ? 1.0f / m_inertia : 0; 287 | } 288 | 289 | Body::BodyPair::BodyPairID Body::BodyPair::generateBodyPairID(Body* bodyA, Body* bodyB) 290 | { 291 | assert(bodyA != nullptr && bodyB != nullptr); 292 | //Combine two 32-bit id into one 64-bit id in binary form 293 | //By Convention: bodyA.id < bodyB.id 294 | auto bodyAId = bodyA->id(); 295 | auto bodyBId = bodyB->id(); 296 | if(bodyAId > bodyBId) 297 | std::swap(bodyAId, bodyBId); 298 | 299 | auto pair = std::pair{ bodyAId, bodyBId }; 300 | auto result = reinterpret_cast(pair); 301 | return result; 302 | } 303 | 304 | Body::BodyPair Body::BodyPair::generateBodyPair(Body* bodyA, Body* bodyB) 305 | { 306 | assert(bodyA != nullptr && bodyB != nullptr); 307 | Body::BodyPair result; 308 | auto bodyAId = bodyA->id(); 309 | auto bodyBId = bodyB->id(); 310 | if (bodyAId > bodyBId) 311 | std::swap(bodyA, bodyB); 312 | 313 | result.pairID = generateBodyPairID(bodyA, bodyB); 314 | result.bodyA = bodyA; 315 | result.bodyB = bodyB; 316 | return result; 317 | } 318 | 319 | 320 | 321 | void Body::PhysicsAttribute::step(const real& dt) 322 | { 323 | position += velocity * dt; 324 | rotation += angularVelocity * dt; 325 | } 326 | 327 | } 328 | -------------------------------------------------------------------------------- /Physics2D/source/dynamics/physics2d_system.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_system.h" 2 | namespace Physics2D 3 | { 4 | int& PhysicsSystem::positionIteration() 5 | { 6 | return m_positionIteration; 7 | } 8 | 9 | int& PhysicsSystem::velocityIteration() 10 | { 11 | return m_velocityIteration; 12 | } 13 | bool& PhysicsSystem::sliceDeltaTime() 14 | { 15 | return m_sliceDeltaTime; 16 | } 17 | 18 | bool& PhysicsSystem::solveJointVelocity() 19 | { 20 | return m_solveJointVelocity; 21 | } 22 | 23 | bool& PhysicsSystem::solveJointPosition() 24 | { 25 | return m_solveJointPosition; 26 | } 27 | 28 | bool& PhysicsSystem::solveContactVelocity() 29 | { 30 | return m_solveContactVelocity; 31 | } 32 | 33 | bool& PhysicsSystem::solveContactPosition() 34 | { 35 | return m_solveContactPosition; 36 | } 37 | 38 | 39 | PhysicsWorld &PhysicsSystem::world() 40 | { 41 | return m_world; 42 | } 43 | 44 | ContactMaintainer &PhysicsSystem::maintainer() 45 | { 46 | return m_maintainer; 47 | } 48 | 49 | Tree &PhysicsSystem::tree() 50 | { 51 | return m_tree; 52 | } 53 | 54 | UniformGrid& PhysicsSystem::grid() 55 | { 56 | return m_grid; 57 | } 58 | 59 | void PhysicsSystem::step(const real &dt) 60 | { 61 | //updateTree(); 62 | //updateGrid(); 63 | 64 | 65 | //solve ccd first, then solve normal case. 66 | if(!solveCCD(dt)) 67 | solve(dt); 68 | 69 | updateTree(); 70 | } 71 | void PhysicsSystem::updateTree() 72 | { 73 | //bvh 74 | for (const auto& elem : m_world.bodyList()) 75 | m_tree.update(elem.get()); 76 | } 77 | 78 | void PhysicsSystem::updateGrid() 79 | { 80 | m_grid.updateAll(); 81 | } 82 | 83 | bool PhysicsSystem::solveCCD(const real& dt) 84 | { 85 | Container::Vector bullets; 86 | for (const auto& body : m_world.bodyList()) 87 | if (body->type() == Body::BodyType::Bullet) 88 | bullets.emplace_back(body.get()); 89 | 90 | for (auto& bullet : bullets) 91 | { 92 | //check bullet velocity threshold 93 | if (bullet->velocity().lengthSquare() < Constant::CCDMinVelocity && bullet->angularVelocity() < Constant::CCDMinVelocity) 94 | continue; 95 | auto potentials = CCD::query(m_tree, bullet, dt); 96 | if (potentials.has_value()) 97 | { 98 | auto finals = CCD::earliestTOI(potentials.value()); 99 | if (finals.has_value()) 100 | { 101 | //if toi still exist, just keep solving them until the sum of toi is greater than dt 102 | real toi = finals.value(); 103 | updateTree(); 104 | solve(toi); 105 | real ddt = (dt - toi) / real(Constant::CCDMaxIterations); 106 | for (int i = 0; i < Constant::CCDMaxIterations; ++i) { 107 | updateTree(); 108 | solve(ddt); 109 | } 110 | //return solved 111 | return true; 112 | } 113 | } 114 | } 115 | //there isn't a ccd case, solve nothing. 116 | return false; 117 | } 118 | void PhysicsSystem::solve(const real& dt) 119 | { 120 | 121 | //Sweep And Prune 122 | 123 | //Container::Vector bodies; 124 | //bodies.reserve(m_world.bodyList().size()); 125 | //for(auto&& elem: m_world.bodyList()) 126 | // bodies.emplace_back(elem.get()); 127 | // 128 | //auto potentialList = SweepAndPrune::generate(bodies); 129 | 130 | 131 | //BVH 132 | real vdt = dt; 133 | real pdt = dt; 134 | if(m_sliceDeltaTime) 135 | { 136 | vdt = dt / real(m_velocityIteration); 137 | pdt = dt / real(m_positionIteration); 138 | } 139 | 140 | m_world.stepVelocity(dt); 141 | //auto potentialList = m_grid.generate(); 142 | 143 | auto potentialList = m_tree.generate(); 144 | for (auto pair : potentialList) 145 | { 146 | auto result = Detector::detect(pair.first, pair.second); 147 | if (result.isColliding) { 148 | m_maintainer.add(result); 149 | } 150 | } 151 | m_maintainer.clearInactivePoints(); 152 | 153 | m_world.prepareVelocityConstraint(dt); 154 | 155 | for (int i = 0; i < m_velocityIteration; ++i) 156 | { 157 | if(m_solveJointVelocity) 158 | m_world.solveVelocityConstraint(vdt); 159 | 160 | if (m_solveContactVelocity) 161 | m_maintainer.solveVelocity(vdt); 162 | } 163 | m_world.stepPosition(dt); 164 | 165 | //solve penetration use contact pairs from previous velocity solver settings 166 | //TODO: Can generate another contact table just for position solving 167 | for (int i = 0; i < m_positionIteration; ++i) 168 | { 169 | if(m_solveContactPosition) 170 | m_maintainer.solvePosition(pdt); 171 | 172 | if(m_solveJointPosition) 173 | m_world.solvePositionConstraint(pdt); 174 | } 175 | 176 | m_maintainer.deactivateAllPoints(); 177 | } 178 | } 179 | -------------------------------------------------------------------------------- /Physics2D/source/math/physics2d_integrator.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_integrator.h" 2 | 3 | namespace Physics2D 4 | { 5 | 6 | } 7 | -------------------------------------------------------------------------------- /Physics2D/source/math/physics2d_math.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_math.h" -------------------------------------------------------------------------------- /Physics2D/source/math/physics2d_matrix2x2.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_matrix2x2.h" 2 | 3 | namespace Physics2D 4 | { 5 | 6 | Matrix2x2::Matrix2x2(const real& radian) 7 | { 8 | set(radian); 9 | } 10 | 11 | 12 | Matrix2x2::Matrix2x2(const Vector2& col1, const Vector2& col2) : column1(col1), column2(col2) 13 | { 14 | } 15 | 16 | Matrix2x2::Matrix2x2(const real& col1_x, const real& col1_y, const real& col2_x, const real& col2_y) 17 | : column1(col1_x, col1_y), column2(col2_x, col2_y) 18 | { 19 | } 20 | 21 | Matrix2x2::Matrix2x2(const Matrix2x2& mat) : column1(mat.column1), column2(mat.column2) 22 | { 23 | } 24 | 25 | Matrix2x2& Matrix2x2::operator=(const Matrix2x2& rhs) 26 | { 27 | if (&rhs == this) 28 | return *this; 29 | column1 = rhs.column1; 30 | column2 = rhs.column2; 31 | return *this; 32 | } 33 | 34 | Matrix2x2& Matrix2x2::operator+=(const Matrix2x2& rhs) 35 | { 36 | column1 += rhs.column1; 37 | column2 += rhs.column2; 38 | return *this; 39 | } 40 | 41 | Matrix2x2& Matrix2x2::operator-=(const Matrix2x2& rhs) 42 | { 43 | column1 -= rhs.column1; 44 | column2 -= rhs.column2; 45 | return *this; 46 | } 47 | 48 | Matrix2x2& Matrix2x2::operator*=(const real& factor) 49 | { 50 | column1 *= factor; 51 | column2 *= factor; 52 | return *this; 53 | } 54 | 55 | Matrix2x2& Matrix2x2::operator/=(const real& factor) 56 | { 57 | assert(!realEqual(factor, 0)); 58 | column1 /= factor; 59 | column2 /= factor; 60 | return *this; 61 | } 62 | 63 | Matrix2x2 Matrix2x2::operator+(const Matrix2x2& rhs) const 64 | { 65 | return Matrix2x2(column1 + rhs.column1, column2 + rhs.column2); 66 | } 67 | 68 | Matrix2x2 Matrix2x2::operator-(const Matrix2x2& rhs) const 69 | { 70 | return Matrix2x2(column1 - rhs.column1, column2 - rhs.column2); 71 | } 72 | 73 | Vector2 Matrix2x2::row1() const 74 | { 75 | return Vector2(column1.x, column2.x); 76 | } 77 | 78 | Vector2 Matrix2x2::row2() const 79 | { 80 | return Vector2(column1.y, column2.y); 81 | } 82 | 83 | real& Matrix2x2::e11() 84 | { 85 | return column1.x; 86 | } 87 | 88 | real& Matrix2x2::e21() 89 | { 90 | return column1.y; 91 | } 92 | real& Matrix2x2::e12() 93 | { 94 | return column2.x; 95 | } 96 | 97 | real& Matrix2x2::e22() 98 | { 99 | return column2.y; 100 | } 101 | 102 | real Matrix2x2::determinant() const 103 | { 104 | return determinant(*this); 105 | } 106 | 107 | Matrix2x2& Matrix2x2::transpose() 108 | { 109 | realSwap(column1.y, column2.x); 110 | return *this; 111 | } 112 | 113 | Matrix2x2& Matrix2x2::invert() 114 | { 115 | invert(*this); 116 | return *this; 117 | } 118 | 119 | Matrix2x2& Matrix2x2::multiply(const Matrix2x2& rhs) 120 | { 121 | *this = multiply(*this, rhs); 122 | return *this; 123 | } 124 | 125 | Vector2 Matrix2x2::multiply(const Vector2& rhs) const 126 | { 127 | return multiply(*this, rhs); 128 | } 129 | 130 | Matrix2x2& Matrix2x2::clear() 131 | { 132 | column1.clear(); 133 | column2.clear(); 134 | return *this; 135 | } 136 | 137 | Matrix2x2& Matrix2x2::set(const real& col1_x, const real& col1_y, const real& col2_x, const real& col2_y) 138 | { 139 | column1.set(col1_x, col1_y); 140 | column2.set(col2_x, col2_y); 141 | return *this; 142 | } 143 | 144 | Matrix2x2& Matrix2x2::set(const Vector2& col1, const Vector2& col2) 145 | { 146 | column1 = col1; 147 | column2 = col2; 148 | return *this; 149 | } 150 | 151 | Matrix2x2& Matrix2x2::set(const Matrix2x2& other) 152 | { 153 | column1 = other.column1; 154 | column2 = other.column2; 155 | return *this; 156 | } 157 | 158 | Matrix2x2& Matrix2x2::set(const real& radian) 159 | { 160 | const real c = Math::cosx(radian); 161 | const real s = Math::sinx(radian); 162 | column1.set(c, s); 163 | column2.set(-s, c); 164 | return *this; 165 | } 166 | 167 | Matrix2x2 Matrix2x2::skewSymmetricMatrix(const Vector2& r) 168 | { 169 | return Matrix2x2(0, -r.y, r.x, 0); 170 | } 171 | 172 | Matrix2x2 Matrix2x2::identityMatrix() 173 | { 174 | return Matrix2x2(1, 0, 0, 1); 175 | } 176 | 177 | Vector2 Matrix2x2::multiply(const Matrix2x2& lhs, const Vector2& rhs) 178 | { 179 | return Vector2(lhs.column1.x * rhs.x + lhs.column2.x * rhs.y, lhs.column1.y * rhs.x + lhs.column2.y * rhs.y); 180 | } 181 | 182 | Matrix2x2 Matrix2x2::multiply(const Matrix2x2& lhs, const Matrix2x2& rhs) 183 | { 184 | return Matrix2x2(lhs.column1.x * rhs.column1.x + lhs.column2.x * rhs.column1.y, 185 | lhs.column1.y * rhs.column1.x + lhs.column2.y * rhs.column1.y, 186 | lhs.column1.x * rhs.column2.x + lhs.column2.x * rhs.column2.y, 187 | lhs.column1.y * rhs.column2.x + lhs.column2.y * rhs.column2.y); 188 | } 189 | 190 | real Matrix2x2::determinant(const Matrix2x2& mat) 191 | { 192 | return mat.column1.x * mat.column2.y - mat.column2.x * mat.column1.y; 193 | } 194 | 195 | bool Matrix2x2::invert(Matrix2x2& mat) 196 | { 197 | const real det = mat.determinant(); 198 | 199 | if (realEqual(det, 0)) 200 | return false; 201 | 202 | realSwap(mat.column1.x, mat.column2.y); 203 | mat.column1.y *= -1; 204 | mat.column2.x *= -1; 205 | mat /= det; 206 | return true; 207 | } 208 | } 209 | -------------------------------------------------------------------------------- /Physics2D/source/math/physics2d_matrix3x3.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_matrix3x3.h" 2 | 3 | namespace Physics2D 4 | { 5 | Matrix3x3::Matrix3x3(const Matrix3x3& mat) : column1(mat.column1), column2(mat.column2), column3(mat.column3) 6 | { 7 | } 8 | 9 | 10 | Matrix3x3::Matrix3x3(const Vector3& col1, const Vector3& col2, const Vector3& col3) 11 | : column1(col1), column2(col2), column3(col3) 12 | { 13 | } 14 | 15 | Matrix3x3::Matrix3x3(const real& col1_x, const real& col1_y, const real& col1_z, const real& col2_x, 16 | const real& col2_y, const real& col2_z, const real& col3_x, const real& col3_y, 17 | const real& col3_z) 18 | : column1(col1_x, col1_y, col1_z), column2(col2_x, col2_y, col2_z), column3(col3_x, col3_y, col3_z) 19 | { 20 | } 21 | 22 | Matrix3x3& Matrix3x3::operator=(const Matrix3x3& rhs) 23 | { 24 | if (&rhs == this) 25 | return *this; 26 | column1 = rhs.column1; 27 | column2 = rhs.column2; 28 | column3 = rhs.column3; 29 | return *this; 30 | } 31 | 32 | Matrix3x3& Matrix3x3::operator+=(const Matrix3x3& rhs) 33 | { 34 | column1 += rhs.column1; 35 | column2 += rhs.column2; 36 | column3 += rhs.column3; 37 | return *this; 38 | } 39 | 40 | Matrix3x3& Matrix3x3::operator-=(const Matrix3x3& rhs) 41 | { 42 | column1 -= rhs.column1; 43 | column2 -= rhs.column2; 44 | column3 -= rhs.column3; 45 | return *this; 46 | } 47 | 48 | Matrix3x3& Matrix3x3::operator*=(const real& factor) 49 | { 50 | column1 *= factor; 51 | column2 *= factor; 52 | column3 *= factor; 53 | return *this; 54 | } 55 | 56 | Matrix3x3& Matrix3x3::operator/=(const real& factor) 57 | { 58 | assert(!realEqual(factor, 0)); 59 | column1 /= factor; 60 | column2 /= factor; 61 | column3 /= factor; 62 | return *this; 63 | } 64 | 65 | Vector3 Matrix3x3::row1() const 66 | { 67 | return Vector3(column1.x, column2.x, column3.x); 68 | } 69 | 70 | Vector3 Matrix3x3::row2() const 71 | { 72 | return Vector3(column1.y, column2.y, column3.y); 73 | } 74 | 75 | Vector3 Matrix3x3::row3() const 76 | { 77 | return Vector3(column1.z, column2.z, column3.z); 78 | } 79 | 80 | real& Matrix3x3::e11() 81 | { 82 | return column1.x; 83 | } 84 | 85 | real& Matrix3x3::e12() 86 | { 87 | return column2.x; 88 | } 89 | 90 | real& Matrix3x3::e13() 91 | { 92 | return column3.x; 93 | } 94 | 95 | real& Matrix3x3::e21() 96 | { 97 | return column1.y; 98 | } 99 | 100 | real& Matrix3x3::e22() 101 | { 102 | return column2.y; 103 | } 104 | 105 | real& Matrix3x3::e23() 106 | { 107 | return column3.y; 108 | } 109 | 110 | real& Matrix3x3::e31() 111 | { 112 | return column1.z; 113 | } 114 | 115 | real& Matrix3x3::e32() 116 | { 117 | return column2.z; 118 | } 119 | 120 | real& Matrix3x3::e33() 121 | { 122 | return column3.z; 123 | } 124 | 125 | real Matrix3x3::determinant() const 126 | { 127 | return determinant(*this); 128 | } 129 | 130 | Matrix3x3& Matrix3x3::transpose() 131 | { 132 | realSwap(column1.y, column2.x); 133 | realSwap(column1.z, column3.x); 134 | realSwap(column2.z, column3.y); 135 | return *this; 136 | } 137 | 138 | Matrix3x3& Matrix3x3::invert() 139 | { 140 | invert(*this); 141 | return *this; 142 | } 143 | 144 | Matrix3x3& Matrix3x3::clear() 145 | { 146 | column1.clear(); 147 | column2.clear(); 148 | column3.clear(); 149 | return *this; 150 | } 151 | 152 | Matrix3x3& Matrix3x3::set(const real& col1_x, const real& col1_y, const real& col1_z, const real& col2_x, 153 | const real& col2_y, const real& col2_z, const real& col3_x, const real& col3_y, 154 | const real& col3_z) 155 | { 156 | column1.set(col1_x, col1_y, col1_z); 157 | column2.set(col2_x, col2_y, col2_z); 158 | column3.set(col3_x, col3_y, col3_z); 159 | return *this; 160 | } 161 | 162 | Matrix3x3& Matrix3x3::set(const Vector3& col1, const Vector3& col2, const Vector3& col3) 163 | { 164 | column1 = col1; 165 | column2 = col2; 166 | column3 = col3; 167 | return *this; 168 | } 169 | 170 | Matrix3x3& Matrix3x3::set(const Matrix3x3& other) 171 | { 172 | column1 = other.column1; 173 | column2 = other.column2; 174 | column3 = other.column3; 175 | return *this; 176 | } 177 | 178 | Vector3 Matrix3x3::multiply(const Vector3& rhs) const 179 | { 180 | return multiply(*this, rhs); 181 | } 182 | 183 | Matrix3x3& Matrix3x3::multiply(const Matrix3x3& rhs) 184 | { 185 | *this = multiply(*this, rhs); 186 | return *this; 187 | } 188 | 189 | Matrix3x3 Matrix3x3::skewSymmetricMatrix(const Vector3& v) 190 | { 191 | return Matrix3x3( 192 | 0, v.z, -v.y, 193 | -v.z, 0, v.x, 194 | v.y, -v.x, 0); 195 | } 196 | 197 | Matrix3x3 Matrix3x3::identityMatrix() 198 | { 199 | return Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1); 200 | } 201 | 202 | Matrix3x3 Matrix3x3::multiply(const Matrix3x3& lhs, const Matrix3x3& rhs) 203 | { 204 | return Matrix3x3(multiply(lhs, rhs.column1), 205 | multiply(lhs, rhs.column2), 206 | multiply(lhs, rhs.column3)); 207 | } 208 | 209 | Vector3 Matrix3x3::multiply(const Matrix3x3& lhs, const Vector3& rhs) 210 | { 211 | return Vector3(lhs.column1.x * rhs.x + lhs.column2.x * rhs.y + lhs.column3.x * rhs.z, 212 | lhs.column1.y * rhs.x + lhs.column2.y * rhs.y + lhs.column3.y * rhs.z, 213 | lhs.column1.z * rhs.x + lhs.column2.z * rhs.y + lhs.column3.z * rhs.z); 214 | } 215 | 216 | real Matrix3x3::determinant(const Matrix3x3& mat) 217 | { 218 | return mat.column1.x * Vector2::crossProduct(mat.column2.y, mat.column2.z, mat.column3.y, mat.column3.z) + 219 | mat.column2.x * Vector2::crossProduct(mat.column3.y, mat.column3.z, mat.column1.y, mat.column1.z) + 220 | mat.column3.x * Vector2::crossProduct(mat.column1.y, mat.column1.z, mat.column2.y, mat.column2.z); 221 | } 222 | 223 | bool Matrix3x3::invert(Matrix3x3& mat) 224 | { 225 | const real det = mat.determinant(); 226 | if (realEqual(det, 0.0f)) 227 | return false; 228 | 229 | const real det11 = Vector2::crossProduct(mat.column2.y, mat.column2.z, mat.column3.y, mat.column3.z); 230 | const real det12 = Vector2::crossProduct(mat.column2.x, mat.column2.z, mat.column3.x, mat.column3.z) * -1; 231 | const real det13 = Vector2::crossProduct(mat.column2.x, mat.column2.y, mat.column3.x, mat.column3.y); 232 | 233 | const real det21 = Vector2::crossProduct(mat.column1.y, mat.column1.z, mat.column3.y, mat.column3.z) * -1; 234 | const real det22 = Vector2::crossProduct(mat.column1.x, mat.column1.z, mat.column3.x, mat.column3.z); 235 | const real det23 = Vector2::crossProduct(mat.column1.x, mat.column1.y, mat.column3.x, mat.column3.y) * -1; 236 | 237 | const real det31 = Vector2::crossProduct(mat.column1.y, mat.column1.z, mat.column2.y, mat.column2.z); 238 | const real det32 = Vector2::crossProduct(mat.column1.x, mat.column1.z, mat.column2.x, mat.column2.z) * -1; 239 | const real det33 = Vector2::crossProduct(mat.column1.x, mat.column1.y, mat.column2.x, mat.column2.y); 240 | 241 | mat.set(det11, det12, det13, det21, det22, det23, det31, det32, det33); 242 | mat.transpose(); 243 | mat /= det; 244 | return true; 245 | } 246 | } 247 | -------------------------------------------------------------------------------- /Physics2D/source/math/physics2d_quaternion.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_quaternion.h" 2 | 3 | namespace Physics2D 4 | { 5 | 6 | } -------------------------------------------------------------------------------- /Physics2D/source/math/physics2d_vector2.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_vector2.h" 2 | #include "physics2d_math.h" 3 | namespace Physics2D 4 | { 5 | Vector2::Vector2(const real& _x, const real& _y) : x(_x), y(_y) 6 | { 7 | assert(!std::isnan(x)); 8 | assert(!std::isnan(y)); 9 | } 10 | 11 | Vector2::Vector2(const Vector2& copy) : x(copy.x), y(copy.y) 12 | { 13 | assert(!std::isnan(x)); 14 | assert(!std::isnan(y)); 15 | } 16 | 17 | Vector2 Vector2::operator+(const Vector2& rhs) const 18 | { 19 | return Vector2(x + rhs.x, y + rhs.y); 20 | } 21 | 22 | Vector2 Vector2::operator-(const Vector2& rhs) const 23 | { 24 | return Vector2(x - rhs.x, y - rhs.y); 25 | } 26 | Vector2 Vector2::operator-()const 27 | { 28 | return Vector2(-x, -y); 29 | } 30 | Vector2 Vector2::operator*(const int& factor) const 31 | { 32 | return Vector2(x * factor, y * factor); 33 | } 34 | 35 | Vector2 Vector2::operator/(const real& factor) const 36 | { 37 | assert(!realEqual(factor, 0)); 38 | return Vector2(x / factor, y / factor); 39 | } 40 | 41 | Vector2& Vector2::operator+=(const Vector2& rhs) 42 | { 43 | x += rhs.x; 44 | y += rhs.y; 45 | return *this; 46 | } 47 | 48 | Vector2& Vector2::operator-=(const Vector2& rhs) 49 | { 50 | x -= rhs.x; 51 | y -= rhs.y; 52 | return *this; 53 | } 54 | 55 | Vector2& Vector2::operator*=(const real& factor) 56 | { 57 | x *= factor; 58 | y *= factor; 59 | return *this; 60 | } 61 | 62 | Vector2& Vector2::operator/=(const real& factor) 63 | { 64 | assert(!realEqual(factor, 0)); 65 | x /= factor; 66 | y /= factor; 67 | return *this; 68 | } 69 | 70 | bool Vector2::operator==(const Vector2& rhs) const 71 | { 72 | return realEqual(x, rhs.x) && realEqual(y, rhs.y); 73 | } 74 | 75 | bool Vector2::operator!=(const Vector2& rhs) const 76 | { 77 | return !realEqual(x, rhs.x) || !realEqual(y, rhs.y); 78 | } 79 | 80 | real Vector2::lengthSquare() const 81 | { 82 | return x * x + y * y; 83 | } 84 | 85 | real Vector2::length() const 86 | { 87 | return std::sqrt(lengthSquare()); 88 | } 89 | 90 | real Vector2::theta() const 91 | { 92 | return Math::arctanx(y, x); 93 | } 94 | 95 | Vector2& Vector2::set(const real& _x, const real& _y) 96 | { 97 | x = _x; 98 | y = _y; 99 | return *this; 100 | } 101 | 102 | Vector2& Vector2::set(const Vector2& copy) 103 | { 104 | x = copy.x; 105 | y = copy.y; 106 | return *this; 107 | } 108 | 109 | Vector2& Vector2::clear() 110 | { 111 | x = 0.0f; 112 | y = 0.0f; 113 | return *this; 114 | } 115 | 116 | Vector2& Vector2::negate() 117 | { 118 | x *= -1.0f; 119 | y *= -1.0f; 120 | return *this; 121 | } 122 | 123 | Vector2& Vector2::swap(Vector2& other) noexcept 124 | { 125 | realSwap(x, other.x); 126 | realSwap(y, other.y); 127 | return *this; 128 | } 129 | 130 | Vector2& Vector2::normalize() 131 | { 132 | const real length_inv = 1.0f / std::sqrt(lengthSquare()); 133 | assert(!std::isinf(length_inv)); 134 | // 135 | 136 | //const real length_inv = Math::fastInverseSqrt(lengthSquare()); 137 | x *= length_inv; 138 | y *= length_inv; 139 | 140 | return *this; 141 | } 142 | 143 | Vector2 Vector2::normal() const 144 | { 145 | return Vector2(*this).normalize(); 146 | } 147 | 148 | Vector2 Vector2::negative() const 149 | { 150 | return Vector2(-x, -y); 151 | } 152 | 153 | bool Vector2::equal(const Vector2& rhs) const 154 | { 155 | return realEqual(x, rhs.x) && realEqual(y, rhs.y); 156 | } 157 | 158 | bool Vector2::fuzzyEqual(const Vector2& rhs, const real& epsilon)const 159 | { 160 | return fuzzyRealEqual(x, rhs.x, epsilon) && fuzzyRealEqual(y, rhs.y, epsilon); 161 | } 162 | 163 | bool Vector2::isOrigin(const real& epsilon) const 164 | { 165 | return fuzzyEqual({ 0, 0 }, epsilon); 166 | } 167 | 168 | bool Vector2::isSameQuadrant(const Vector2& rhs) const 169 | { 170 | return Math::sameSign(x, rhs.x) && Math::sameSign(y, rhs.y); 171 | } 172 | 173 | real Vector2::dot(const Vector2& rhs) const 174 | { 175 | return x * rhs.x + y * rhs.y; 176 | } 177 | 178 | real Vector2::cross(const Vector2& rhs) const 179 | { 180 | return x * rhs.y - y * rhs.x; 181 | } 182 | 183 | Vector2& Vector2::matchSign(const Vector2& rhs) 184 | { 185 | x = std::abs(x); 186 | y = std::abs(y); 187 | if (rhs.x < 0) 188 | x = -x; 189 | if (rhs.y < 0) 190 | y = -y; 191 | return *this; 192 | } 193 | 194 | Vector2 Vector2::perpendicular() const 195 | { 196 | return Vector2(-y, x); 197 | } 198 | 199 | real Vector2::dotProduct(const Vector2& lhs, const Vector2& rhs) 200 | { 201 | return lhs.x * rhs.x + lhs.y * rhs.y; 202 | } 203 | 204 | real Vector2::crossProduct(const Vector2& lhs, const Vector2& rhs) 205 | { 206 | return lhs.x * rhs.y - lhs.y * rhs.x; 207 | } 208 | 209 | real Vector2::crossProduct(const real& x1, const real& y1, const real& x2, const real& y2) 210 | { 211 | return x1 * y2 - x2 * y1; 212 | } 213 | 214 | Vector2 Vector2::crossProduct(const real& lhs, const Vector2& rhs) 215 | { 216 | return Vector2(-rhs.y, rhs.x) * lhs; 217 | } 218 | 219 | Vector2 Vector2::crossProduct(const Vector2& lhs, const real& rhs) 220 | { 221 | return Vector2(lhs.y, -lhs.x) * rhs; 222 | } 223 | 224 | Vector2 Vector2::lerp(const Vector2& lhs, const Vector2& rhs, const real& t) 225 | { 226 | return lhs + (rhs - lhs) * t; 227 | } 228 | 229 | Vector2& Vector2::operator/=(const int& factor) 230 | { 231 | assert(!realEqual(factor, 0)); 232 | x /= factor; 233 | y /= factor; 234 | return *this; 235 | } 236 | 237 | Vector2& Vector2::operator*=(const int& factor) 238 | { 239 | x *= factor; 240 | y *= factor; 241 | return *this; 242 | } 243 | 244 | Vector2 Vector2::operator/(const int& factor) const 245 | { 246 | assert(!realEqual(factor, 0)); 247 | return Vector2(x / factor, y / factor); 248 | } 249 | 250 | Vector2 Vector2::operator*(const real& factor) const 251 | { 252 | return Vector2(x * factor, y * factor); 253 | } 254 | 255 | Vector2& Vector2::operator=(const Vector2& copy) 256 | { 257 | if (© == this) 258 | return *this; 259 | x = copy.x; 260 | y = copy.y; 261 | return *this; 262 | } 263 | } 264 | -------------------------------------------------------------------------------- /Physics2D/source/math/physics2d_vector3.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_vector3.h" 2 | #include "physics2d_math.h" 3 | 4 | namespace Physics2D 5 | { 6 | Vector3::Vector3(const real& _x, const real& _y, const real& _z) : x(_x), y(_y), z(_z) 7 | { 8 | } 9 | 10 | Vector3::Vector3(const Vector3& copy) : x(copy.x), y(copy.y), z(copy.z) 11 | { 12 | } 13 | 14 | Vector3 Vector3::operator+(const Vector3& rhs) const 15 | { 16 | return Vector3(x + rhs.x, y + rhs.y, z + rhs.z); 17 | } 18 | 19 | Vector3 Vector3::operator-(const Vector3& other) const 20 | { 21 | return Vector3(x - other.x, y - other.y, z - other.z); 22 | } 23 | 24 | Vector3 Vector3::operator-() const 25 | { 26 | return Vector3(-x, -y, -z); 27 | } 28 | 29 | Vector3 Vector3::operator*(const real& factor) const 30 | { 31 | return Vector3(x * factor, y * factor, z * factor); 32 | } 33 | 34 | Vector3 Vector3::operator/(const real& factor) const 35 | { 36 | assert(!realEqual(factor, 0)); 37 | return Vector3(x / factor, y / factor, z / factor); 38 | } 39 | 40 | Vector3& Vector3::operator+=(const Vector3& rhs) 41 | { 42 | x += rhs.x; 43 | y += rhs.y; 44 | z += rhs.z; 45 | return *this; 46 | } 47 | 48 | Vector3& Vector3::operator-=(const Vector3& rhs) 49 | { 50 | x -= rhs.x; 51 | y -= rhs.y; 52 | z -= rhs.z; 53 | return *this; 54 | } 55 | 56 | Vector3& Vector3::operator*=(const real& factor) 57 | { 58 | x *= factor; 59 | y *= factor; 60 | z *= factor; 61 | return *this; 62 | } 63 | 64 | Vector3& Vector3::operator/=(const real& factor) 65 | { 66 | assert(!realEqual(factor, 0)); 67 | x /= factor; 68 | y /= factor; 69 | z /= factor; 70 | return *this; 71 | } 72 | 73 | Vector3& Vector3::set(const real& _x, const real& _y, const real& _z) 74 | { 75 | x = _x; 76 | y = _y; 77 | z = _z; 78 | return *this; 79 | } 80 | 81 | Vector3& Vector3::set(const Vector3& other) 82 | { 83 | x = other.x; 84 | y = other.y; 85 | z = other.z; 86 | return *this; 87 | } 88 | 89 | Vector3& Vector3::clear() 90 | { 91 | x = 0.0f; 92 | y = 0.0f; 93 | z = 0.0f; 94 | return *this; 95 | } 96 | 97 | Vector3 Vector3::negative() const 98 | { 99 | return Vector3(-x, -y, -z); 100 | } 101 | Vector3& Vector3::negate() 102 | { 103 | x = -x; 104 | y = -y; 105 | z = -z; 106 | return *this; 107 | } 108 | 109 | real Vector3::lengthSquare() const 110 | { 111 | return x * x + y * y + z * z; 112 | } 113 | 114 | real Vector3::length() const 115 | { 116 | return sqrt(lengthSquare()); 117 | } 118 | 119 | Vector3& Vector3::normalize() 120 | { 121 | const real length_inv = Math::fastInverseSqrt(lengthSquare()); 122 | x *= length_inv; 123 | y *= length_inv; 124 | z *= length_inv; 125 | return *this; 126 | } 127 | 128 | Vector3 Vector3::normal() const 129 | { 130 | return Vector3(*this).normalize(); 131 | } 132 | 133 | bool Vector3::equal(const Vector3& rhs) const 134 | { 135 | return realEqual(x, rhs.x) && realEqual(y, rhs.y) && realEqual(z, rhs.z); 136 | } 137 | 138 | bool Vector3::fuzzyEqual(const Vector3& rhs, const real& epsilon) const 139 | { 140 | return (*this - rhs).lengthSquare() < epsilon; 141 | } 142 | bool Vector3::isOrigin(const real& epsilon) const 143 | { 144 | return fuzzyEqual({0, 0, 0}, epsilon); 145 | } 146 | Vector3& Vector3::swap(Vector3& other) 147 | { 148 | realSwap(x, other.x); 149 | realSwap(y, other.y); 150 | realSwap(z, other.z); 151 | return *this; 152 | } 153 | 154 | real Vector3::dot(const Vector3& rhs) const 155 | { 156 | return x * rhs.x + y * rhs.y + z * rhs.z; 157 | } 158 | 159 | Vector3& Vector3::cross(const Vector3& rhs) 160 | { 161 | x = y * rhs.z - rhs.y * z; 162 | y = rhs.x * z - x * rhs.z; 163 | z = x * rhs.y - y * rhs.x; 164 | return *this; 165 | } 166 | 167 | real Vector3::dotProduct(const Vector3& lhs, const Vector3& rhs) 168 | { 169 | return lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z; 170 | } 171 | 172 | Vector3 Vector3::crossProduct(const Vector3& lhs, const Vector3& rhs) 173 | { 174 | return Vector3(lhs.y * rhs.z - rhs.y * lhs.z, rhs.x * lhs.z - lhs.x * rhs.z, lhs.x * rhs.y - lhs.y * rhs.x); 175 | } 176 | 177 | Vector3& Vector3::operator/=(const int& factor) 178 | { 179 | assert(!realEqual(factor, 0)); 180 | x /= factor; 181 | y /= factor; 182 | z /= factor; 183 | return *this; 184 | } 185 | 186 | Vector3& Vector3::operator*=(const int& factor) 187 | { 188 | x *= factor; 189 | y *= factor; 190 | z *= factor; 191 | return *this; 192 | } 193 | 194 | Vector3 Vector3::operator/(const int& factor) const 195 | { 196 | assert(!realEqual(factor, 0)); 197 | return Vector3(x / factor, y / factor, z / factor); 198 | } 199 | 200 | Vector3 Vector3::operator*(const int& factor) const 201 | { 202 | return Vector3(x * factor, y * factor, z * factor); 203 | } 204 | 205 | Vector3& Vector3::operator=(const Vector3& copy) 206 | { 207 | if (© == this) 208 | return *this; 209 | x = copy.x; 210 | y = copy.y; 211 | z = copy.z; 212 | return *this; 213 | } 214 | } 215 | -------------------------------------------------------------------------------- /Physics2D/source/math/physics2d_vector4.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_vector4.h" 2 | #include "physics2d_math.h" 3 | 4 | namespace Physics2D 5 | { 6 | 7 | Vector4::Vector4(const real& _x, const real& _y, const real& _z, const real& _w) 8 | : x(_x), y(_y), z(_z), w(_w) 9 | { 10 | } 11 | 12 | Vector4& Vector4::operator=(const Vector4& copy) 13 | { 14 | if (© == this) 15 | return *this; 16 | x = copy.x; 17 | y = copy.y; 18 | z = copy.z; 19 | w = copy.w; 20 | return *this; 21 | } 22 | 23 | Vector4& Vector4::operator=(const Vector3& copy) 24 | { 25 | x = copy.x; 26 | y = copy.y; 27 | z = copy.z; 28 | w = 0.0; 29 | return *this; 30 | } 31 | 32 | Vector4::Vector4(const Vector3& copy) 33 | :x(copy.x), y(copy.y), z(copy.z), w(0.0) 34 | { 35 | 36 | } 37 | 38 | Vector4 Vector4::operator+(const Vector4& rhs) const 39 | { 40 | return Vector4(x + rhs.x, y + rhs.y, z + rhs.z, w + rhs.w); 41 | } 42 | 43 | Vector4 Vector4::operator-(const Vector4& other) const 44 | { 45 | return Vector4(x - other.x, y - other.y, z - other.z, w - other.w); 46 | } 47 | Vector4 Vector4::operator-() const 48 | { 49 | return Vector4(-x, -y, -z, -w); 50 | } 51 | Vector4 Vector4::operator*(const real& factor) const 52 | { 53 | return Vector4(x * factor, y * factor, z * factor, w * factor); 54 | } 55 | 56 | Vector4 Vector4::operator/(const real& factor) const 57 | { 58 | 59 | assert(!realEqual(factor, 0)); 60 | return Vector3(x / factor, y / factor, z / factor); 61 | } 62 | 63 | Vector4& Vector4::operator+=(const Vector4& rhs) 64 | { 65 | x += rhs.x; 66 | y += rhs.y; 67 | z += rhs.z; 68 | w += rhs.w; 69 | return *this; 70 | } 71 | 72 | Vector4& Vector4::operator-=(const Vector4& rhs) 73 | { 74 | x -= rhs.x; 75 | y -= rhs.y; 76 | z -= rhs.z; 77 | w -= rhs.w; 78 | return *this; 79 | } 80 | 81 | Vector4& Vector4::operator*=(const real& factor) 82 | { 83 | x *= factor; 84 | y *= factor; 85 | z *= factor; 86 | w *= factor; 87 | return *this; 88 | } 89 | 90 | Vector4& Vector4::operator/=(const real& factor) 91 | { 92 | assert(!realEqual(factor, 0)); 93 | x /= factor; 94 | y /= factor; 95 | z /= factor; 96 | w /= factor; 97 | return *this; 98 | } 99 | 100 | Vector4& Vector4::set(const real& _x, const real& _y, const real& _z, const real& _w) 101 | { 102 | x = _x; 103 | y = _y; 104 | z = _z; 105 | w = _w; 106 | return *this; 107 | } 108 | 109 | 110 | Vector4& Vector4::set(const Vector4& other) 111 | { 112 | x = other.x; 113 | y = other.y; 114 | z = other.z; 115 | w = other.w; 116 | return *this; 117 | } 118 | 119 | Vector4& Vector4::set(const Vector3& other) 120 | { 121 | x = other.x; 122 | y = other.y; 123 | z = other.z; 124 | w = 0.0; 125 | return *this; 126 | } 127 | 128 | Vector4& Vector4::clear() 129 | { 130 | x = 0.0; 131 | y = 0.0; 132 | z = 0.0; 133 | w = 0.0; 134 | return *this; 135 | } 136 | 137 | Vector4& Vector4::negate() 138 | { 139 | x = -x; 140 | y = -y; 141 | z = -z; 142 | w = -w; 143 | return *this; 144 | } 145 | 146 | Vector4& Vector4::normalize() 147 | { 148 | const real length_inv = Math::fastInverseSqrt(lengthSquare()); 149 | x *= length_inv; 150 | y *= length_inv; 151 | z *= length_inv; 152 | w *= length_inv; 153 | return *this; 154 | } 155 | 156 | real Vector4::lengthSquare() const 157 | { 158 | return x * x + y * y + z * z + w * w; 159 | } 160 | 161 | real Vector4::length() const 162 | { 163 | return sqrt(lengthSquare()); 164 | } 165 | 166 | Vector4 Vector4::normal() const 167 | { 168 | return Vector4(*this).normalize(); 169 | } 170 | 171 | Vector4 Vector4::negative() const 172 | { 173 | return Vector4(-x, -y, -z, -w); 174 | } 175 | 176 | bool Vector4::equal(const Vector4& rhs) const 177 | { 178 | return realEqual(x, rhs.x) && realEqual(y, rhs.y) 179 | && realEqual(z, rhs.z) && realEqual(w, rhs.w); 180 | } 181 | 182 | bool Vector4::fuzzyEqual(const Vector4& rhs, const real& epsilon) const 183 | { 184 | return (*this - rhs).lengthSquare() < epsilon; 185 | } 186 | 187 | bool Vector4::isOrigin(const real& epsilon) const 188 | { 189 | return fuzzyEqual({ 0, 0, 0 , 0}, epsilon); 190 | } 191 | 192 | Vector4& Vector4::swap(Vector4& other) 193 | { 194 | realSwap(x, other.x); 195 | realSwap(y, other.y); 196 | realSwap(z, other.z); 197 | realSwap(w, other.w); 198 | return *this; 199 | } 200 | 201 | real Vector4::dot(const Vector4& rhs) const 202 | { 203 | return x * rhs.x + y * rhs.y + z * rhs.z + w * rhs.w; 204 | } 205 | 206 | real Vector4::dotProduct(const Vector4& lhs, const Vector4& rhs) 207 | { 208 | return lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z + lhs.w * rhs.w; 209 | } 210 | 211 | } 212 | -------------------------------------------------------------------------------- /Physics2D/source/other/physics2d_common.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_common.h" -------------------------------------------------------------------------------- /Physics2D/source/other/physics2d_random.cpp: -------------------------------------------------------------------------------- 1 | #include "physics2d_random.h" 2 | 3 | namespace Physics2D 4 | { 5 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Physics2D 2 | [中文](README_zh_CN.md) 3 | 4 | Simple 2D Physics Engine For Blog Tutorials. 5 | 6 | > Attention: For testbed, please see [Physics2D-TestBed-SFML](https://github.com/AngryAccelerated/Physics2D-TestBed-SFML) . 7 | 8 | # Build 9 | 10 | Use [XMake](https://github.com/xmake-io/xmake) to build project: 11 | 12 | ``` 13 | xmake build 14 | ``` 15 | 16 | # Requirement 17 | - C++ 20 18 | 19 | # Features 20 | - Basic Linear Algebra 21 | - Collision Detection 22 | - Narrowphase 23 | - Algorithm 24 | - SAT 25 | - GJK & EPA & MPR & Distance 26 | - Contact Pair By Sutherland-Hogdman Clipping 27 | - Continuous Collision Detection 28 | - Sampling Trajectory of Body 29 | - Time of Impact 30 | - Broadphase 31 | - Axis-Aligned Bounding Box 32 | - Dynamic Bounding Volume Tree 33 | - SAH 34 | - Dynamic Tree & Array 35 | - Raycast 36 | - Sweep And Prune 37 | - Spatial Hashing Grid 38 | - Contact Maintainer 39 | - Rigid Body Dynamics Simulation 40 | - Sequential Impulse Solver 41 | - Joint 42 | - Distance 43 | - Rotation 44 | - Point 45 | - Mouse 46 | - Simple 2D Geometry Algorithm 47 | - Support Mapping 48 | - Ellipse 49 | - Circle 50 | - Polygon 51 | - Line 52 | - Point 53 | - Capsule 54 | - Sector 55 | - Intersection 56 | - Raycast 57 | - Line Segment 58 | - Convexity 59 | - Graham Scan 60 | - Convexity Test 61 | - Center 62 | - Incenter 63 | - Centroid 64 | - Circumcenter 65 | - Circle 66 | - Circumcircle 67 | - Inscribed-circle 68 | - Ellipse 69 | - Support Mapping 70 | - Nearest Point 71 | 72 | # Future 73 | - Integrator 74 | - Verlet 75 | - Rk4 76 | - Joint 77 | - Prismatic 78 | - Weld 79 | - Non-Fit Polygon 80 | - Soft Body 81 | - Finite Element Method 82 | - Mass-Spring System 83 | - Rope 84 | - Position-Based Dynamics 85 | 86 | # Reference 87 | - [Box2D](https://github.com/erincatto/box2d) 88 | - [Box2D Lite](https://github.com/erincatto/box2d-lite) 89 | - [dyn4j](https://github.com/dyn4j/dyn4j) 90 | - [matterjs](https://github.com/liabru/matter-js) 91 | - [nphysics](https://github.com/dimforge/nphysics) 92 | - [Box2D Publications](https://box2d.org/publications/) 93 | - [dyn4j Official Blog](https://dyn4j.org/blog/) 94 | - [Game Physics For Beginners - liabru](https://brm.io/game-physics-for-beginners/) 95 | - [Allen Chou's Blog](http://allenchou.net/game-physics-series/) 96 | - [Physics Constraints Series - Allen Chou](https://www.youtube.com/c/MingLunChou/videos) 97 | - [Soft Constraints - ODE](https://ode.org/ode-latest-userguide.html#sec_3_8_0) 98 | - [Gaffer's on Games](https://gafferongames.com/#posts) 99 | - [Randy Gaul's Blog](https://randygaul.github.io/) 100 | - [Winter's Dev](https://blog.winter.dev/) 101 | - [Primitives and Intersection Acceleration](https://www.pbr-book.org/3ed-2018/Primitives_and_Intersection_Acceleration/Bounding_Volume_Hierarchies) 102 | - [Real-Time Rendering Intersection](http://www.realtimerendering.com/intersections.html) 103 | - [Inigo Quilez's 2D SDF Functions](https://www.iquilezles.org/www/articles/distfunctions2d/distfunctions2d.htm) 104 | - [*A Simple Time-Corrected Verlet Integration Method* - Jonathan Dummer](https://archive.gamedev.net/archive/reference/programming/features/verlet/) 105 | - [*Introduction to rigid body pipeline, collision detection* - Erwin Coumans](https://docs.google.com/presentation/d/1wGUJ4neOhw5i4pQRfSGtZPE3CIm7MfmqfTp5aJKuFYM/edit#slide=id.g644a5aa5f_1_116) 106 | - *Foundations of Physically Based Modeling and Animation* - Donald House and John C. Keyser 107 | - *Real-Time Collision Detection* by Christer Ericson 108 | - *Game Programming Gems 7* - Scott Jacobs 109 | - *Game Physics Cookbook* - Gabor Szauer 110 | -------------------------------------------------------------------------------- /README_zh_CN.md: -------------------------------------------------------------------------------- 1 | # Physics2D 2 | 3 | 一个简单、用于博客教学的 2D 游戏物理引擎。 4 | 知乎专栏：[ACRL's Development](https://www.zhihu.com/column/c_1262755781494808576) 5 | 6 | > 请注意: 如果要尝试效果，请前往 [Physics2D-TestBed-SFML](https://github.com/AngryAccelerated/Physics2D-TestBed-SFML) 。 7 | 8 | # 快速构建 9 | 10 | 使用 [XMake](https://github.com/xmake-io/xmake) 进行项目构建: 11 | 12 | ``` 13 | xmake build 14 | ``` 15 | 16 | # 编译环境 17 | - C++ 20 18 | 19 | # 功能特点 20 | - 基本的线性代数类 21 | - 2D 图元的碰撞检测 22 | - 精检测阶段（Narrowphase） 23 | - 检测算法 24 | - 分离轴算法 25 | - GJK 算法 26 | - 多边形扩展算法 27 | - 闵可夫斯基入口简化 28 | - Sutherland Hodgman 多边形裁剪 29 | - 连续碰撞检测 30 | - 轨迹采样 31 | - 冲击时间 32 | - 粗检测阶段 33 | - 轴对齐包围盒 34 | - 动态层次包围体树 35 | - 表面积启发法 36 | - 光线物体查询 37 | - 动态树与数组 38 | - 扫掠减除法 39 | - 均匀网格法 40 | - 碰撞点维护 41 | - 刚体模拟 42 | - 连续冲力解算器 43 | - 约束关节 44 | - 鼠标约束 45 | - 旋转/朝向约束 46 | - 点约束 47 | - 距离约束 48 | - 测试 Demo 49 | - 基本 Debug 绘图 50 | - 2D 平滑相机 51 | - 缩放 52 | - 平滑移动 53 | - 跟踪物体 54 | - 基本 2D 计算几何算法 55 | - 基本图元映射 56 | - 相交测试 57 | - 凸体检测 58 | - 三角形三心计算 59 | - 外/内接圆 60 | - 椭圆最近点查询 61 | 62 | # 计划清单 63 | - 积分器 64 | - 韦尔莱 65 | - 四阶龙格库塔法 66 | - 关节约束 67 | - 结合约束 68 | - 软体模拟 69 | - 有限元方法 70 | - 质点弹簧系统 71 | - 绳子模拟 72 | - 基于位置的动力学 73 | 74 | # 参考 75 | - [Box2D](https://github.com/erincatto/box2d) 76 | - [Box2D Lite](https://github.com/erincatto/box2d-lite) 77 | - [dyn4j](https://github.com/dyn4j/dyn4j) 78 | - [matterjs](https://github.com/liabru/matter-js) 79 | - [nphysics](https://github.com/dimforge/nphysics) 80 | - [Box2D Publications](https://box2d.org/publications/) 81 | - [dyn4j Official Blog](https://dyn4j.org/blog/) 82 | - [Game Physics For Beginners - liabru](https://brm.io/game-physics-for-beginners/) 83 | - [Allen Chou's Blog](http://allenchou.net/game-physics-series/) 84 | - [Physics Constraints Series - Allen Chou](https://www.youtube.com/c/MingLunChou/videos) 85 | - [Soft Constraints - ODE](https://ode.org/ode-latest-userguide.html#sec_3_8_0) 86 | - [Gaffer's on Games](https://gafferongames.com/#posts) 87 | - [Randy Gaul's Blog](http://www.randygaul.net/) 88 | - [Winter's Dev](https://blog.winter.dev/) 89 | - [Primitives and Intersection Acceleration](https://www.pbr-book.org/3ed-2018/Primitives_and_Intersection_Acceleration/Bounding_Volume_Hierarchies) 90 | - [Real-Time Rendering Intersection](http://www.realtimerendering.com/intersections.html) 91 | - [Inigo Quilez's 2D SDF Functions](https://www.iquilezles.org/www/articles/distfunctions2d/distfunctions2d.htm) 92 | - [*A Simple Time-Corrected Verlet Integration Method* - Jonathan Dummer](https://archive.gamedev.net/archive/reference/programming/features/verlet/) 93 | - [*Introduction to rigid body pipeline, collision detection* - Erwin Coumans](https://docs.google.com/presentation/d/1wGUJ4neOhw5i4pQRfSGtZPE3CIm7MfmqfTp5aJKuFYM/edit#slide=id.g644a5aa5f_1_116) 94 | - *Foundations of Physically Based Modeling and Animation* - Donald House and John C. Keyser 95 | - *Real-Time Collision Detection* by Christer Ericson 96 | - *Game Programming Gems 7* - Scott Jacobs 97 | - *Game Physics Cookbook* - Gabor Szauer 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /xmake.lua: -------------------------------------------------------------------------------- 1 | add_rules("mode.debug", "mode.release", "mode.profile") 2 | set_languages("c++20") 3 | 4 | target("Physics2D") 5 | if is_mode("debug") then 6 | set_kind("static") 7 | add_defines("DEBUG") 8 | set_symbols("debug") 9 | set_optimize("none") 10 | end 11 | if is_mode("release", "profile") then 12 | set_kind("shared") 13 | add_defines("NDEBUG") 14 | set_symbols("hidden") 15 | set_optimize("fastest") 16 | end 17 | 18 | add_headerfiles("Physics2D/include/*.h") 19 | add_includedirs("Physics2D/include") 20 | add_files("Physics2D/source/collision/*.cpp") 21 | add_files("Physics2D/source/dynamics/*.cpp") 22 | add_files("Physics2D/source/math/*.cpp") 23 | add_files("Physics2D/source/other/*.cpp") --------------------------------------------------------------------------------