diff --git a/platform/macosx/love.xcodeproj/project.pbxproj b/platform/macosx/love.xcodeproj/project.pbxproj index 933c8eeaa..77cba2653 100644 --- a/platform/macosx/love.xcodeproj/project.pbxproj +++ b/platform/macosx/love.xcodeproj/project.pbxproj @@ -147,6 +147,22 @@ A968F0D91083A9FC00A895AA /* Mouse.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A968F0D81083A9FC00A895AA /* Mouse.cpp */; }; A968F0DD1083AA6600A895AA /* Shape.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A968F0DB1083AA6600A895AA /* Shape.cpp */; }; A968F0DE1083AA6600A895AA /* Joint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A968F0DC1083AA6600A895AA /* Joint.cpp */; }; + A96F416014127E340067FE9A /* b2RopeJoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A96F415C14127E310067FE9A /* b2RopeJoint.cpp */; }; + A96F416114127E340067FE9A /* b2WheelJoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A96F415E14127E320067FE9A /* b2WheelJoint.cpp */; }; + A96F416514127E720067FE9A /* b2Rope.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A96F416314127E720067FE9A /* b2Rope.cpp */; }; + A96F416B14127E9C0067FE9A /* b2Draw.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A96F416614127E9C0067FE9A /* b2Draw.cpp */; }; + A96F416C14127E9C0067FE9A /* b2Timer.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A96F416914127E9C0067FE9A /* b2Timer.cpp */; }; + A96F416E14127EB20067FE9A /* b2CollideEdge.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A96F416D14127EB20067FE9A /* b2CollideEdge.cpp */; }; + A96F417114127EC00067FE9A /* b2ChainShape.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A96F416F14127EC00067FE9A /* b2ChainShape.cpp */; }; + A96F417614127EE70067FE9A /* b2ChainAndCircleContact.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A96F417214127EE70067FE9A /* b2ChainAndCircleContact.cpp */; }; + A96F417714127EE70067FE9A /* b2ChainAndPolygonContact.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A96F417414127EE70067FE9A /* b2ChainAndPolygonContact.cpp */; }; + A96F417C14127EFD0067FE9A /* b2EdgeAndCircleContact.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A96F417814127EFD0067FE9A /* b2EdgeAndCircleContact.cpp */; }; + A96F417D14127EFD0067FE9A /* b2EdgeAndPolygonContact.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A96F417A14127EFD0067FE9A /* b2EdgeAndPolygonContact.cpp */; }; + A96F418014127FC20067FE9A /* b2EdgeShape.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A96F417E14127FC20067FE9A /* b2EdgeShape.cpp */; }; + A96F41831412AFE20067FE9A /* wrap_WeldJoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A96F41811412AFB70067FE9A /* wrap_WeldJoint.cpp */; }; + A96F41861412AFF80067FE9A /* WeldJoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A96F41841412AFEB0067FE9A /* WeldJoint.cpp */; }; + A96F41891412B36D0067FE9A /* wrap_WheelJoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A96F41871412B35B0067FE9A /* wrap_WheelJoint.cpp */; }; + A96F418C1412B3AD0067FE9A /* WheelJoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A96F418A1412B3A30067FE9A /* WheelJoint.cpp */; }; A986DEB5113249A800810279 /* Thread.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A986DEB1113249A800810279 /* Thread.cpp */; }; A986DEB6113249A800810279 /* wrap_Thread.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A986DEB3113249A800810279 /* wrap_Thread.cpp */; }; A986EC64132CE6DB00F048C8 /* b2BroadPhase.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A986EBC8132CE6D800F048C8 /* b2BroadPhase.cpp */; }; @@ -173,12 +189,10 @@ A986EC79132CE6DB00F048C8 /* b2ContactSolver.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A986EBF6132CE6D800F048C8 /* b2ContactSolver.cpp */; }; A986EC7A132CE6DB00F048C8 /* b2PolygonAndCircleContact.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A986EBF8132CE6D800F048C8 /* b2PolygonAndCircleContact.cpp */; }; A986EC7B132CE6DB00F048C8 /* b2PolygonContact.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A986EBFA132CE6D800F048C8 /* b2PolygonContact.cpp */; }; - A986EC7C132CE6DB00F048C8 /* b2TOISolver.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A986EBFC132CE6D800F048C8 /* b2TOISolver.cpp */; }; A986EC7D132CE6DB00F048C8 /* b2DistanceJoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A986EBFF132CE6D800F048C8 /* b2DistanceJoint.cpp */; }; A986EC7E132CE6DB00F048C8 /* b2FrictionJoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A986EC01132CE6D800F048C8 /* b2FrictionJoint.cpp */; }; A986EC7F132CE6DB00F048C8 /* b2GearJoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A986EC03132CE6D800F048C8 /* b2GearJoint.cpp */; }; A986EC80132CE6DB00F048C8 /* b2Joint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A986EC05132CE6D800F048C8 /* b2Joint.cpp */; }; - A986EC81132CE6DB00F048C8 /* b2LineJoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A986EC07132CE6D800F048C8 /* b2LineJoint.cpp */; }; A986EC82132CE6DB00F048C8 /* b2MouseJoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A986EC09132CE6D800F048C8 /* b2MouseJoint.cpp */; }; A986EC83132CE6DB00F048C8 /* b2PrismaticJoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A986EC0B132CE6D800F048C8 /* b2PrismaticJoint.cpp */; }; A986EC84132CE6DB00F048C8 /* b2PulleyJoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A986EC0D132CE6D800F048C8 /* b2PulleyJoint.cpp */; }; @@ -188,6 +202,8 @@ A98D914410507C97008E03F2 /* EncodedImageData.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A98D914310507C97008E03F2 /* EncodedImageData.cpp */; }; A9B4BA9C1045937F001DBC80 /* ParticleSystem.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A9B4BA9A1045937F001DBC80 /* ParticleSystem.cpp */; }; A9B4BA9D1045937F001DBC80 /* wrap_ParticleSystem.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A9B4BA9B1045937F001DBC80 /* wrap_ParticleSystem.cpp */; }; + A9B6432C13BF83AB00DC3C7E /* FrictionJoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A9B6432A13BF833900DC3C7E /* FrictionJoint.cpp */; }; + A9B6432F13BF87F100DC3C7E /* wrap_FrictionJoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A9B6432D13BF87E400DC3C7E /* wrap_FrictionJoint.cpp */; }; A9BD60741226C988007DEC63 /* Framebuffer.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A9BD60701226C988007DEC63 /* Framebuffer.cpp */; }; A9BD60751226C988007DEC63 /* wrap_Framebuffer.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A9BD60721226C988007DEC63 /* wrap_Framebuffer.cpp */; }; A9CF0E8610B9EB1000E6F37E /* utf8.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A9CF0E8510B9EB1000E6F37E /* utf8.cpp */; }; @@ -523,6 +539,38 @@ A968F0D81083A9FC00A895AA /* Mouse.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = Mouse.cpp; sourceTree = ""; }; A968F0DB1083AA6600A895AA /* Shape.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = Shape.cpp; sourceTree = ""; }; A968F0DC1083AA6600A895AA /* Joint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = Joint.cpp; sourceTree = ""; }; + A96F415C14127E310067FE9A /* b2RopeJoint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = b2RopeJoint.cpp; path = ../../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Dynamics/Joints/b2RopeJoint.cpp; sourceTree = ""; }; + A96F415D14127E320067FE9A /* b2RopeJoint.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = b2RopeJoint.h; path = ../../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Dynamics/Joints/b2RopeJoint.h; sourceTree = ""; }; + A96F415E14127E320067FE9A /* b2WheelJoint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = b2WheelJoint.cpp; path = ../../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Dynamics/Joints/b2WheelJoint.cpp; sourceTree = ""; }; + A96F415F14127E320067FE9A /* b2WheelJoint.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = b2WheelJoint.h; path = ../../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Dynamics/Joints/b2WheelJoint.h; sourceTree = ""; }; + A96F416314127E720067FE9A /* b2Rope.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = b2Rope.cpp; sourceTree = ""; }; + A96F416414127E720067FE9A /* b2Rope.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2Rope.h; sourceTree = ""; }; + A96F416614127E9C0067FE9A /* b2Draw.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = b2Draw.cpp; path = ../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Common/b2Draw.cpp; sourceTree = ""; }; + A96F416714127E9C0067FE9A /* b2Draw.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = b2Draw.h; path = ../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Common/b2Draw.h; sourceTree = ""; }; + A96F416814127E9C0067FE9A /* b2GrowableStack.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = b2GrowableStack.h; path = ../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Common/b2GrowableStack.h; sourceTree = ""; }; + A96F416914127E9C0067FE9A /* b2Timer.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = b2Timer.cpp; path = ../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Common/b2Timer.cpp; sourceTree = ""; }; + A96F416A14127E9C0067FE9A /* b2Timer.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = b2Timer.h; path = ../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Common/b2Timer.h; sourceTree = ""; }; + A96F416D14127EB20067FE9A /* b2CollideEdge.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = b2CollideEdge.cpp; path = ../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Collision/b2CollideEdge.cpp; sourceTree = ""; }; + A96F416F14127EC00067FE9A /* b2ChainShape.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = b2ChainShape.cpp; path = ../../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Collision/Shapes/b2ChainShape.cpp; sourceTree = ""; }; + A96F417014127EC00067FE9A /* b2ChainShape.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = b2ChainShape.h; path = ../../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Collision/Shapes/b2ChainShape.h; sourceTree = ""; }; + A96F417214127EE70067FE9A /* b2ChainAndCircleContact.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = b2ChainAndCircleContact.cpp; path = ../../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp; sourceTree = ""; }; + A96F417314127EE70067FE9A /* b2ChainAndCircleContact.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = b2ChainAndCircleContact.h; path = ../../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h; sourceTree = ""; }; + A96F417414127EE70067FE9A /* b2ChainAndPolygonContact.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = b2ChainAndPolygonContact.cpp; path = ../../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp; sourceTree = ""; }; + A96F417514127EE70067FE9A /* b2ChainAndPolygonContact.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = b2ChainAndPolygonContact.h; path = ../../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h; sourceTree = ""; }; + A96F417814127EFD0067FE9A /* b2EdgeAndCircleContact.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = b2EdgeAndCircleContact.cpp; path = ../../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp; sourceTree = ""; }; + A96F417914127EFD0067FE9A /* b2EdgeAndCircleContact.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = b2EdgeAndCircleContact.h; path = ../../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h; sourceTree = ""; }; + A96F417A14127EFD0067FE9A /* b2EdgeAndPolygonContact.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = b2EdgeAndPolygonContact.cpp; path = ../../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp; sourceTree = ""; }; + A96F417B14127EFD0067FE9A /* b2EdgeAndPolygonContact.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = b2EdgeAndPolygonContact.h; path = ../../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h; sourceTree = ""; }; + A96F417E14127FC20067FE9A /* b2EdgeShape.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = b2EdgeShape.cpp; path = ../../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Collision/Shapes/b2EdgeShape.cpp; sourceTree = ""; }; + A96F417F14127FC20067FE9A /* b2EdgeShape.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = b2EdgeShape.h; path = ../../../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Collision/Shapes/b2EdgeShape.h; sourceTree = ""; }; + A96F41811412AFB70067FE9A /* wrap_WeldJoint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = wrap_WeldJoint.cpp; sourceTree = ""; }; + A96F41821412AFC60067FE9A /* wrap_WeldJoint.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = wrap_WeldJoint.h; sourceTree = ""; }; + A96F41841412AFEB0067FE9A /* WeldJoint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = WeldJoint.cpp; sourceTree = ""; }; + A96F41851412AFEE0067FE9A /* WeldJoint.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = WeldJoint.h; sourceTree = ""; }; + A96F41871412B35B0067FE9A /* wrap_WheelJoint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = wrap_WheelJoint.cpp; sourceTree = ""; }; + A96F41881412B3610067FE9A /* wrap_WheelJoint.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = wrap_WheelJoint.h; sourceTree = ""; }; + A96F418A1412B3A30067FE9A /* WheelJoint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = WheelJoint.cpp; sourceTree = ""; }; + A96F418B1412B3A80067FE9A /* WheelJoint.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = WheelJoint.h; sourceTree = ""; }; A97E3842132A9EDE00198A2F /* love-Info.plist */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.plist.xml; path = "love-Info.plist"; sourceTree = ""; }; A986DEB1113249A800810279 /* Thread.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = Thread.cpp; sourceTree = ""; }; A986DEB2113249A800810279 /* Thread.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = Thread.h; sourceTree = ""; }; @@ -576,8 +624,6 @@ A986EBF9132CE6D800F048C8 /* b2PolygonAndCircleContact.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2PolygonAndCircleContact.h; sourceTree = ""; }; A986EBFA132CE6D800F048C8 /* b2PolygonContact.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = b2PolygonContact.cpp; sourceTree = ""; }; A986EBFB132CE6D800F048C8 /* b2PolygonContact.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2PolygonContact.h; sourceTree = ""; }; - A986EBFC132CE6D800F048C8 /* b2TOISolver.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = b2TOISolver.cpp; sourceTree = ""; }; - A986EBFD132CE6D800F048C8 /* b2TOISolver.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2TOISolver.h; sourceTree = ""; }; A986EBFF132CE6D800F048C8 /* b2DistanceJoint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = b2DistanceJoint.cpp; sourceTree = ""; }; A986EC00132CE6D800F048C8 /* b2DistanceJoint.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2DistanceJoint.h; sourceTree = ""; }; A986EC01132CE6D800F048C8 /* b2FrictionJoint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = b2FrictionJoint.cpp; sourceTree = ""; }; @@ -586,8 +632,6 @@ A986EC04132CE6D800F048C8 /* b2GearJoint.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2GearJoint.h; sourceTree = ""; }; A986EC05132CE6D800F048C8 /* b2Joint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = b2Joint.cpp; sourceTree = ""; }; A986EC06132CE6D800F048C8 /* b2Joint.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2Joint.h; sourceTree = ""; }; - A986EC07132CE6D800F048C8 /* b2LineJoint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = b2LineJoint.cpp; sourceTree = ""; }; - A986EC08132CE6D800F048C8 /* b2LineJoint.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2LineJoint.h; sourceTree = ""; }; A986EC09132CE6D800F048C8 /* b2MouseJoint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = b2MouseJoint.cpp; sourceTree = ""; }; A986EC0A132CE6D800F048C8 /* b2MouseJoint.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = b2MouseJoint.h; sourceTree = ""; }; A986EC0B132CE6D800F048C8 /* b2PrismaticJoint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = b2PrismaticJoint.cpp; sourceTree = ""; }; @@ -608,6 +652,10 @@ A9B4BA991045937F001DBC80 /* wrap_ParticleSystem.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = wrap_ParticleSystem.h; sourceTree = ""; }; A9B4BA9A1045937F001DBC80 /* ParticleSystem.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = ParticleSystem.cpp; sourceTree = ""; }; A9B4BA9B1045937F001DBC80 /* wrap_ParticleSystem.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = wrap_ParticleSystem.cpp; sourceTree = ""; }; + A9B6432A13BF833900DC3C7E /* FrictionJoint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = FrictionJoint.cpp; sourceTree = ""; }; + A9B6432B13BF837800DC3C7E /* FrictionJoint.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = FrictionJoint.h; sourceTree = ""; }; + A9B6432D13BF87E400DC3C7E /* wrap_FrictionJoint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = wrap_FrictionJoint.cpp; sourceTree = ""; }; + A9B6432E13BF87EA00DC3C7E /* wrap_FrictionJoint.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = wrap_FrictionJoint.h; sourceTree = ""; }; A9BD60701226C988007DEC63 /* Framebuffer.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = Framebuffer.cpp; sourceTree = ""; }; A9BD60711226C988007DEC63 /* Framebuffer.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = Framebuffer.h; sourceTree = ""; }; A9BD60721226C988007DEC63 /* wrap_Framebuffer.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = wrap_Framebuffer.cpp; sourceTree = ""; }; @@ -1149,6 +1197,8 @@ A93E6AF210420AC7007D418B /* DistanceJoint.h */, A986ECAD132CEB9300F048C8 /* Fixture.cpp */, A986ECAE132CEB9B00F048C8 /* Fixture.h */, + A9B6432A13BF833900DC3C7E /* FrictionJoint.cpp */, + A9B6432B13BF837800DC3C7E /* FrictionJoint.h */, A93E6AF310420AC7007D418B /* GearJoint.cpp */, A93E6AF410420AC7007D418B /* GearJoint.h */, A93E6AF510420AC7007D418B /* graham */, @@ -1168,6 +1218,10 @@ A93E6B0710420AC8007D418B /* RevoluteJoint.h */, A93E6B0810420AC8007D418B /* Shape.cpp */, A93E6B0910420AC8007D418B /* Shape.h */, + A96F41841412AFEB0067FE9A /* WeldJoint.cpp */, + A96F41851412AFEE0067FE9A /* WeldJoint.h */, + A96F418A1412B3A30067FE9A /* WheelJoint.cpp */, + A96F418B1412B3A80067FE9A /* WheelJoint.h */, A93E6B4E10420ACA007D418B /* World.cpp */, A93E6B4F10420ACA007D418B /* World.h */, A93E6B5010420ACA007D418B /* wrap_Body.cpp */, @@ -1178,6 +1232,8 @@ A93E6B5510420ACA007D418B /* wrap_Contact.h */, A93E6B5610420ACA007D418B /* wrap_DistanceJoint.cpp */, A93E6B5710420ACA007D418B /* wrap_DistanceJoint.h */, + A9B6432D13BF87E400DC3C7E /* wrap_FrictionJoint.cpp */, + A9B6432E13BF87EA00DC3C7E /* wrap_FrictionJoint.h */, A93E6B5810420ACA007D418B /* wrap_GearJoint.cpp */, A93E6B5910420ACA007D418B /* wrap_GearJoint.h */, A93E6B5A10420ACA007D418B /* wrap_Joint.cpp */, @@ -1196,6 +1252,10 @@ A93E6B6710420ACA007D418B /* wrap_RevoluteJoint.h */, A93E6B6810420ACA007D418B /* wrap_Shape.cpp */, A93E6B6910420ACA007D418B /* wrap_Shape.h */, + A96F41811412AFB70067FE9A /* wrap_WeldJoint.cpp */, + A96F41821412AFC60067FE9A /* wrap_WeldJoint.h */, + A96F41871412B35B0067FE9A /* wrap_WheelJoint.cpp */, + A96F41881412B3610067FE9A /* wrap_WheelJoint.h */, A93E6B6A10420ACA007D418B /* wrap_World.cpp */, A93E6B6B10420ACB007D418B /* wrap_World.h */, ); @@ -1300,6 +1360,16 @@ path = utf8; sourceTree = ""; }; + A96F416214127E720067FE9A /* Rope */ = { + isa = PBXGroup; + children = ( + A96F416314127E720067FE9A /* b2Rope.cpp */, + A96F416414127E720067FE9A /* b2Rope.h */, + ); + name = Rope; + path = ../../../../../../Downloads/Box2D_v2.2.0/Box2D/Box2D/Rope; + sourceTree = ""; + }; A986DEAF113249A700810279 /* thread */ = { isa = PBXGroup; children = ( @@ -1327,6 +1397,7 @@ A986EBC7132CE6D800F048C8 /* Collision */, A986EBDA132CE6D800F048C8 /* Common */, A986EBE3132CE6D800F048C8 /* Dynamics */, + A96F416214127E720067FE9A /* Rope */, ); path = Box2D; sourceTree = ""; @@ -1337,6 +1408,7 @@ A986EBC8132CE6D800F048C8 /* b2BroadPhase.cpp */, A986EBC9132CE6D800F048C8 /* b2BroadPhase.h */, A986EBCA132CE6D800F048C8 /* b2CollideCircle.cpp */, + A96F416D14127EB20067FE9A /* b2CollideEdge.cpp */, A986EBCB132CE6D800F048C8 /* b2CollidePolygon.cpp */, A986EBCC132CE6D800F048C8 /* b2Collision.cpp */, A986EBCD132CE6D800F048C8 /* b2Collision.h */, @@ -1354,8 +1426,12 @@ A986EBD4132CE6D800F048C8 /* Shapes */ = { isa = PBXGroup; children = ( + A96F416F14127EC00067FE9A /* b2ChainShape.cpp */, + A96F417014127EC00067FE9A /* b2ChainShape.h */, A986EBD5132CE6D800F048C8 /* b2CircleShape.cpp */, A986EBD6132CE6D800F048C8 /* b2CircleShape.h */, + A96F417E14127FC20067FE9A /* b2EdgeShape.cpp */, + A96F417F14127FC20067FE9A /* b2EdgeShape.h */, A986EBD7132CE6D800F048C8 /* b2PolygonShape.cpp */, A986EBD8132CE6D800F048C8 /* b2PolygonShape.h */, A986EBD9132CE6D800F048C8 /* b2Shape.h */, @@ -1368,12 +1444,17 @@ children = ( A986EBDB132CE6D800F048C8 /* b2BlockAllocator.cpp */, A986EBDC132CE6D800F048C8 /* b2BlockAllocator.h */, + A96F416614127E9C0067FE9A /* b2Draw.cpp */, + A96F416714127E9C0067FE9A /* b2Draw.h */, + A96F416814127E9C0067FE9A /* b2GrowableStack.h */, A986EBDD132CE6D800F048C8 /* b2Math.cpp */, A986EBDE132CE6D800F048C8 /* b2Math.h */, A986EBDF132CE6D800F048C8 /* b2Settings.cpp */, A986EBE0132CE6D800F048C8 /* b2Settings.h */, A986EBE1132CE6D800F048C8 /* b2StackAllocator.cpp */, A986EBE2132CE6D800F048C8 /* b2StackAllocator.h */, + A96F416914127E9C0067FE9A /* b2Timer.cpp */, + A96F416A14127E9C0067FE9A /* b2Timer.h */, ); path = Common; sourceTree = ""; @@ -1403,18 +1484,24 @@ A986EBF1132CE6D800F048C8 /* Contacts */ = { isa = PBXGroup; children = ( + A96F417214127EE70067FE9A /* b2ChainAndCircleContact.cpp */, + A96F417314127EE70067FE9A /* b2ChainAndCircleContact.h */, + A96F417414127EE70067FE9A /* b2ChainAndPolygonContact.cpp */, + A96F417514127EE70067FE9A /* b2ChainAndPolygonContact.h */, A986EBF2132CE6D800F048C8 /* b2CircleContact.cpp */, A986EBF3132CE6D800F048C8 /* b2CircleContact.h */, A986EBF4132CE6D800F048C8 /* b2Contact.cpp */, A986EBF5132CE6D800F048C8 /* b2Contact.h */, A986EBF6132CE6D800F048C8 /* b2ContactSolver.cpp */, A986EBF7132CE6D800F048C8 /* b2ContactSolver.h */, + A96F417814127EFD0067FE9A /* b2EdgeAndCircleContact.cpp */, + A96F417914127EFD0067FE9A /* b2EdgeAndCircleContact.h */, + A96F417A14127EFD0067FE9A /* b2EdgeAndPolygonContact.cpp */, + A96F417B14127EFD0067FE9A /* b2EdgeAndPolygonContact.h */, A986EBF8132CE6D800F048C8 /* b2PolygonAndCircleContact.cpp */, A986EBF9132CE6D800F048C8 /* b2PolygonAndCircleContact.h */, A986EBFA132CE6D800F048C8 /* b2PolygonContact.cpp */, A986EBFB132CE6D800F048C8 /* b2PolygonContact.h */, - A986EBFC132CE6D800F048C8 /* b2TOISolver.cpp */, - A986EBFD132CE6D800F048C8 /* b2TOISolver.h */, ); path = Contacts; sourceTree = ""; @@ -1430,8 +1517,6 @@ A986EC04132CE6D800F048C8 /* b2GearJoint.h */, A986EC05132CE6D800F048C8 /* b2Joint.cpp */, A986EC06132CE6D800F048C8 /* b2Joint.h */, - A986EC07132CE6D800F048C8 /* b2LineJoint.cpp */, - A986EC08132CE6D800F048C8 /* b2LineJoint.h */, A986EC09132CE6D800F048C8 /* b2MouseJoint.cpp */, A986EC0A132CE6D800F048C8 /* b2MouseJoint.h */, A986EC0B132CE6D800F048C8 /* b2PrismaticJoint.cpp */, @@ -1440,8 +1525,12 @@ A986EC0E132CE6D800F048C8 /* b2PulleyJoint.h */, A986EC0F132CE6D800F048C8 /* b2RevoluteJoint.cpp */, A986EC10132CE6D800F048C8 /* b2RevoluteJoint.h */, + A96F415C14127E310067FE9A /* b2RopeJoint.cpp */, + A96F415D14127E320067FE9A /* b2RopeJoint.h */, A986EC11132CE6D800F048C8 /* b2WeldJoint.cpp */, A986EC12132CE6D800F048C8 /* b2WeldJoint.h */, + A96F415E14127E320067FE9A /* b2WheelJoint.cpp */, + A96F415F14127E320067FE9A /* b2WheelJoint.h */, ); path = Joints; sourceTree = ""; @@ -1473,6 +1562,9 @@ /* Begin PBXProject section */ 29B97313FDCFA39411CA2CEA /* Project object */ = { isa = PBXProject; + attributes = { + LastUpgradeCheck = 0410; + }; buildConfigurationList = C01FCF4E08A954540054247B /* Build configuration list for PBXProject "love" */; compatibilityVersion = "Xcode 3.1"; developmentRegion = English; @@ -1666,18 +1758,34 @@ A986EC79132CE6DB00F048C8 /* b2ContactSolver.cpp in Sources */, A986EC7A132CE6DB00F048C8 /* b2PolygonAndCircleContact.cpp in Sources */, A986EC7B132CE6DB00F048C8 /* b2PolygonContact.cpp in Sources */, - A986EC7C132CE6DB00F048C8 /* b2TOISolver.cpp in Sources */, A986EC7D132CE6DB00F048C8 /* b2DistanceJoint.cpp in Sources */, A986EC7E132CE6DB00F048C8 /* b2FrictionJoint.cpp in Sources */, A986EC7F132CE6DB00F048C8 /* b2GearJoint.cpp in Sources */, A986EC80132CE6DB00F048C8 /* b2Joint.cpp in Sources */, - A986EC81132CE6DB00F048C8 /* b2LineJoint.cpp in Sources */, A986EC82132CE6DB00F048C8 /* b2MouseJoint.cpp in Sources */, A986EC83132CE6DB00F048C8 /* b2PrismaticJoint.cpp in Sources */, A986EC84132CE6DB00F048C8 /* b2PulleyJoint.cpp in Sources */, A986EC85132CE6DB00F048C8 /* b2RevoluteJoint.cpp in Sources */, A986EC86132CE6DB00F048C8 /* b2WeldJoint.cpp in Sources */, A986ECAF132CEBB000F048C8 /* Fixture.cpp in Sources */, + A9B6432C13BF83AB00DC3C7E /* FrictionJoint.cpp in Sources */, + A9B6432F13BF87F100DC3C7E /* wrap_FrictionJoint.cpp in Sources */, + A96F416014127E340067FE9A /* b2RopeJoint.cpp in Sources */, + A96F416114127E340067FE9A /* b2WheelJoint.cpp in Sources */, + A96F416514127E720067FE9A /* b2Rope.cpp in Sources */, + A96F416B14127E9C0067FE9A /* b2Draw.cpp in Sources */, + A96F416C14127E9C0067FE9A /* b2Timer.cpp in Sources */, + A96F416E14127EB20067FE9A /* b2CollideEdge.cpp in Sources */, + A96F417114127EC00067FE9A /* b2ChainShape.cpp in Sources */, + A96F417614127EE70067FE9A /* b2ChainAndCircleContact.cpp in Sources */, + A96F417714127EE70067FE9A /* b2ChainAndPolygonContact.cpp in Sources */, + A96F417C14127EFD0067FE9A /* b2EdgeAndCircleContact.cpp in Sources */, + A96F417D14127EFD0067FE9A /* b2EdgeAndPolygonContact.cpp in Sources */, + A96F418014127FC20067FE9A /* b2EdgeShape.cpp in Sources */, + A96F41831412AFE20067FE9A /* wrap_WeldJoint.cpp in Sources */, + A96F41861412AFF80067FE9A /* WeldJoint.cpp in Sources */, + A96F41891412B36D0067FE9A /* wrap_WheelJoint.cpp in Sources */, + A96F418C1412B3AD0067FE9A /* WheelJoint.cpp in Sources */, ); runOnlyForDeploymentPostprocessing = 0; }; @@ -1743,6 +1851,7 @@ ); ONLY_ACTIVE_ARCH = NO; PRODUCT_NAME = love; + SDKROOT = macosx; }; name = Release; }; diff --git a/src/common/types.h b/src/common/types.h index 7009baa14..567d93744 100644 --- a/src/common/types.h +++ b/src/common/types.h @@ -72,6 +72,8 @@ namespace love PHYSICS_SHAPE_ID, PHYSICS_CIRCLE_SHAPE_ID, PHYSICS_POLYGON_SHAPE_ID, + PHYSICS_EDGE_SHAPE_ID, + PHYSICS_CHAIN_SHAPE_ID, PHYSICS_JOINT_ID, PHYSICS_MOUSE_JOINT_ID, PHYSICS_DISTANCE_JOINT_ID, @@ -79,6 +81,10 @@ namespace love PHYSICS_REVOLUTE_JOINT_ID, PHYSICS_PULLEY_JOINT_ID, PHYSICS_GEAR_JOINT_ID, + PHYSICS_FRICTION_JOINT_ID, + PHYSICS_WELD_JOINT_ID, + PHYSICS_ROPE_JOINT_ID, + PHYSICS_WHEEL_JOINT_ID, // Thread THREAD_THREAD_ID, @@ -137,6 +143,8 @@ namespace love const bits PHYSICS_SHAPE_T = (bits(1) << PHYSICS_SHAPE_ID) | OBJECT_T; const bits PHYSICS_CIRCLE_SHAPE_T = (bits(1) << PHYSICS_CIRCLE_SHAPE_ID) | PHYSICS_SHAPE_T; const bits PHYSICS_POLYGON_SHAPE_T = (bits(1) << PHYSICS_POLYGON_SHAPE_ID) | PHYSICS_SHAPE_T; + const bits PHYSICS_EDGE_SHAPE_T = (bits(1) << PHYSICS_EDGE_SHAPE_ID) | PHYSICS_SHAPE_T; + const bits PHYSICS_CHAIN_SHAPE_T = (bits(1) << PHYSICS_CHAIN_SHAPE_ID) | PHYSICS_SHAPE_T; const bits PHYSICS_JOINT_T = (bits(1) << PHYSICS_JOINT_ID) | OBJECT_T; const bits PHYSICS_MOUSE_JOINT_T = (bits(1) << PHYSICS_MOUSE_JOINT_ID) | PHYSICS_JOINT_T; const bits PHYSICS_DISTANCE_JOINT_T = (bits(1) << PHYSICS_DISTANCE_JOINT_ID) | PHYSICS_JOINT_T; @@ -144,6 +152,10 @@ namespace love const bits PHYSICS_REVOLUTE_JOINT_T = (bits(1) << PHYSICS_REVOLUTE_JOINT_ID) | PHYSICS_JOINT_T; const bits PHYSICS_PULLEY_JOINT_T = (bits(1) << PHYSICS_PULLEY_JOINT_ID) | PHYSICS_JOINT_T; const bits PHYSICS_GEAR_JOINT_T = (bits(1) << PHYSICS_GEAR_JOINT_ID) | PHYSICS_JOINT_T; + const bits PHYSICS_FRICTION_JOINT_T = (bits(1) << PHYSICS_FRICTION_JOINT_ID) | PHYSICS_JOINT_T; + const bits PHYSICS_WELD_JOINT_T = (bits(1) << PHYSICS_WELD_JOINT_ID) | PHYSICS_JOINT_T; + const bits PHYSICS_ROPE_JOINT_T = (bits(1) << PHYSICS_ROPE_JOINT_ID) | PHYSICS_JOINT_T; + const bits PHYSICS_WHEEL_JOINT_T = (bits(1) << PHYSICS_WHEEL_JOINT_ID) | PHYSICS_JOINT_T; // Thread. const bits THREAD_THREAD_T = (bits(1) << THREAD_THREAD_ID) | OBJECT_T; diff --git a/src/modules/physics/Joint.cpp b/src/modules/physics/Joint.cpp index add6b33bf..f25b61e7d 100644 --- a/src/modules/physics/Joint.cpp +++ b/src/modules/physics/Joint.cpp @@ -47,8 +47,9 @@ namespace physics {"pulley", Joint::JOINT_PULLEY}, {"gear", Joint::JOINT_GEAR}, {"friction", Joint::JOINT_FRICTION}, - {"line", Joint::JOINT_LINE}, {"weld", Joint::JOINT_WELD}, + {"wheel", Joint::JOINT_WHEEL}, + {"rope", Joint::JOINT_ROPE}, }; StringMap Joint::types(Joint::typeEntries, sizeof(Joint::typeEntries)); diff --git a/src/modules/physics/Joint.h b/src/modules/physics/Joint.h index efa89bec3..a37d29350 100644 --- a/src/modules/physics/Joint.h +++ b/src/modules/physics/Joint.h @@ -43,8 +43,9 @@ namespace physics JOINT_PULLEY, JOINT_GEAR, JOINT_FRICTION, - JOINT_LINE, JOINT_WELD, + JOINT_WHEEL, + JOINT_ROPE, JOINT_MAX_ENUM }; diff --git a/src/modules/physics/box2d/Box2D/Box2D.h b/src/modules/physics/box2d/Box2D/Box2D.h old mode 100644 new mode 100755 index dc5701f51..817cd1ccf --- a/src/modules/physics/box2d/Box2D/Box2D.h +++ b/src/modules/physics/box2d/Box2D/Box2D.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -32,8 +32,12 @@ For discussion please visit http://box2d.org/forum // These include files constitute the main Box2D API #include +#include +#include #include +#include +#include #include #include @@ -52,11 +56,14 @@ For discussion please visit http://box2d.org/forum #include #include #include -#include +#include #include #include #include #include +#include #include +#include + #endif diff --git a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2ChainShape.cpp b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2ChainShape.cpp new file mode 100755 index 000000000..f7bbe06b6 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2ChainShape.cpp @@ -0,0 +1,171 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +using namespace std; + +b2ChainShape::~b2ChainShape() +{ + b2Free(m_vertices); + m_vertices = NULL; + m_count = 0; +} + +void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count) +{ + b2Assert(m_vertices == NULL && m_count == 0); + b2Assert(count >= 3); + m_count = count + 1; + m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); + memcpy(m_vertices, vertices, count * sizeof(b2Vec2)); + m_vertices[count] = m_vertices[0]; + m_prevVertex = m_vertices[m_count - 2]; + m_nextVertex = m_vertices[1]; + m_hasPrevVertex = true; + m_hasNextVertex = true; +} + +void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count) +{ + b2Assert(m_vertices == NULL && m_count == 0); + b2Assert(count >= 2); + m_count = count; + m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2)); + memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2)); + m_hasPrevVertex = false; + m_hasNextVertex = false; +} + +void b2ChainShape::SetPrevVertex(const b2Vec2& prevVertex) +{ + m_prevVertex = prevVertex; + m_hasPrevVertex = true; +} + +void b2ChainShape::SetNextVertex(const b2Vec2& nextVertex) +{ + m_nextVertex = nextVertex; + m_hasNextVertex = true; +} + +b2Shape* b2ChainShape::Clone(b2BlockAllocator* allocator) const +{ + void* mem = allocator->Allocate(sizeof(b2ChainShape)); + b2ChainShape* clone = new (mem) b2ChainShape; + clone->CreateChain(m_vertices, m_count); + clone->m_prevVertex = m_prevVertex; + clone->m_nextVertex = m_nextVertex; + clone->m_hasPrevVertex = m_hasPrevVertex; + clone->m_hasNextVertex = m_hasNextVertex; + return clone; +} + +int32 b2ChainShape::GetChildCount() const +{ + // edge count = vertex count - 1 + return m_count - 1; +} + +void b2ChainShape::GetChildEdge(b2EdgeShape* edge, int32 index) const +{ + b2Assert(0 <= index && index < m_count - 1); + edge->m_type = b2Shape::e_edge; + edge->m_radius = m_radius; + + edge->m_vertex1 = m_vertices[index + 0]; + edge->m_vertex2 = m_vertices[index + 1]; + + if (index > 0) + { + edge->m_vertex0 = m_vertices[index - 1]; + edge->m_hasVertex0 = true; + } + else + { + edge->m_vertex0 = m_prevVertex; + edge->m_hasVertex0 = m_hasPrevVertex; + } + + if (index < m_count - 2) + { + edge->m_vertex3 = m_vertices[index + 2]; + edge->m_hasVertex3 = true; + } + else + { + edge->m_vertex3 = m_nextVertex; + edge->m_hasVertex3 = m_hasNextVertex; + } +} + +bool b2ChainShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const +{ + B2_NOT_USED(xf); + B2_NOT_USED(p); + return false; +} + +bool b2ChainShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& xf, int32 childIndex) const +{ + b2Assert(childIndex < m_count); + + b2EdgeShape edgeShape; + + int32 i1 = childIndex; + int32 i2 = childIndex + 1; + if (i2 == m_count) + { + i2 = 0; + } + + edgeShape.m_vertex1 = m_vertices[i1]; + edgeShape.m_vertex2 = m_vertices[i2]; + + return edgeShape.RayCast(output, input, xf, 0); +} + +void b2ChainShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const +{ + b2Assert(childIndex < m_count); + + int32 i1 = childIndex; + int32 i2 = childIndex + 1; + if (i2 == m_count) + { + i2 = 0; + } + + b2Vec2 v1 = b2Mul(xf, m_vertices[i1]); + b2Vec2 v2 = b2Mul(xf, m_vertices[i2]); + + aabb->lowerBound = b2Min(v1, v2); + aabb->upperBound = b2Max(v1, v2); +} + +void b2ChainShape::ComputeMass(b2MassData* massData, float32 density) const +{ + B2_NOT_USED(density); + + massData->mass = 0.0f; + massData->center.SetZero(); + massData->I = 0.0f; +} diff --git a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2ChainShape.h b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2ChainShape.h new file mode 100755 index 000000000..34593e2b8 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2ChainShape.h @@ -0,0 +1,117 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B2_CHAIN_SHAPE_H +#define B2_CHAIN_SHAPE_H + +#include + +class b2EdgeShape; + +/// A chain shape is a free form sequence of line segments. +/// The chain has two-sided collision, so you can use inside and outside collision. +/// Therefore, you may use any winding order. +/// Since there may be many vertices, they are allocated using b2Alloc. +/// Connectivity information is used to create smooth collisions. +/// WARNING: The chain will not collide properly if there are self-intersections. +class b2ChainShape : public b2Shape +{ +public: + b2ChainShape(); + + /// The destructor frees the vertices using b2Free. + ~b2ChainShape(); + + /// Create a loop. This automatically adjusts connectivity. + /// @param vertices an array of vertices, these are copied + /// @param count the vertex count + void CreateLoop(const b2Vec2* vertices, int32 count); + + /// Create a chain with isolated end vertices. + /// @param vertices an array of vertices, these are copied + /// @param count the vertex count + void CreateChain(const b2Vec2* vertices, int32 count); + + /// Establish connectivity to a vertex that precedes the first vertex. + /// Don't call this for loops. + void SetPrevVertex(const b2Vec2& prevVertex); + + /// Establish connectivity to a vertex that follows the last vertex. + /// Don't call this for loops. + void SetNextVertex(const b2Vec2& nextVertex); + + /// Implement b2Shape. Vertices are cloned using b2Alloc. + b2Shape* Clone(b2BlockAllocator* allocator) const; + + /// @see b2Shape::GetChildCount + int32 GetChildCount() const; + + /// Get a child edge. + void GetChildEdge(b2EdgeShape* edge, int32 index) const; + + /// This always return false. + /// @see b2Shape::TestPoint + bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; + + /// Implement b2Shape. + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const; + + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const; + + /// Chains have zero mass. + /// @see b2Shape::ComputeMass + void ComputeMass(b2MassData* massData, float32 density) const; + + /// Get the number of vertices. + int32 GetVertexCount() const { return m_count; } + + /// Get the vertices (read-only). + const b2Vec2& GetVertex(int32 index) const + { + b2Assert(0 <= index && index < m_count); + return m_vertices[index]; + } + + /// Get the vertices (read-only). + const b2Vec2* GetVertices() const { return m_vertices; } + +protected: + + /// The vertices. Owned by this class. + b2Vec2* m_vertices; + + /// The vertex count. + int32 m_count; + + b2Vec2 m_prevVertex, m_nextVertex; + bool m_hasPrevVertex, m_hasNextVertex; +}; + +inline b2ChainShape::b2ChainShape() +{ + m_type = e_chain; + m_radius = b2_polygonRadius; + m_vertices = NULL; + m_count = 0; + m_hasPrevVertex = NULL; + m_hasNextVertex = NULL; +} + +#endif diff --git a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2CircleShape.cpp b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2CircleShape.cpp old mode 100644 new mode 100755 index a950b0be5..c03d662df --- a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2CircleShape.cpp +++ b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2CircleShape.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -18,6 +18,7 @@ #include #include +using namespace std; b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const { @@ -27,9 +28,14 @@ b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const return clone; } +int32 b2CircleShape::GetChildCount() const +{ + return 1; +} + bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const { - b2Vec2 center = transform.position + b2Mul(transform.R, m_p); + b2Vec2 center = transform.p + b2Mul(transform.q, m_p); b2Vec2 d = p - center; return b2Dot(d, d) <= m_radius * m_radius; } @@ -38,9 +44,12 @@ bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) con // From Section 3.1.2 // x = s + a * r // norm(x) = radius -bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const +bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const { - b2Vec2 position = transform.position + b2Mul(transform.R, m_p); + B2_NOT_USED(childIndex); + + b2Vec2 position = transform.p + b2Mul(transform.q, m_p); b2Vec2 s = input.p1 - position; float32 b = b2Dot(s, s) - m_radius * m_radius; @@ -72,9 +81,11 @@ bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input return false; } -void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform) const +void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const { - b2Vec2 p = transform.position + b2Mul(transform.R, m_p); + B2_NOT_USED(childIndex); + + b2Vec2 p = transform.p + b2Mul(transform.q, m_p); aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius); aabb->upperBound.Set(p.x + m_radius, p.y + m_radius); } diff --git a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2CircleShape.h b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2CircleShape.h old mode 100644 new mode 100755 index bb31da898..6c1fd543a --- a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2CircleShape.h +++ b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2CircleShape.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -30,14 +30,18 @@ public: /// Implement b2Shape. b2Shape* Clone(b2BlockAllocator* allocator) const; + /// @see b2Shape::GetChildCount + int32 GetChildCount() const; + /// Implement b2Shape. bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; /// Implement b2Shape. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const; + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const; /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform) const; + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const; /// @see b2Shape::ComputeMass void ComputeMass(b2MassData* massData, float32 density) const; diff --git a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2EdgeShape.cpp b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2EdgeShape.cpp new file mode 100755 index 000000000..e204160b0 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2EdgeShape.cpp @@ -0,0 +1,139 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +using namespace std; + +void b2EdgeShape::Set(const b2Vec2& v1, const b2Vec2& v2) +{ + m_vertex1 = v1; + m_vertex2 = v2; + m_hasVertex0 = false; + m_hasVertex3 = false; +} + +b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const +{ + void* mem = allocator->Allocate(sizeof(b2EdgeShape)); + b2EdgeShape* clone = new (mem) b2EdgeShape; + *clone = *this; + return clone; +} + +int32 b2EdgeShape::GetChildCount() const +{ + return 1; +} + +bool b2EdgeShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const +{ + B2_NOT_USED(xf); + B2_NOT_USED(p); + return false; +} + +// p = p1 + t * d +// v = v1 + s * e +// p1 + t * d = v1 + s * e +// s * e - t * d = p1 - v1 +bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& xf, int32 childIndex) const +{ + B2_NOT_USED(childIndex); + + // Put the ray into the edge's frame of reference. + b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); + b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); + b2Vec2 d = p2 - p1; + + b2Vec2 v1 = m_vertex1; + b2Vec2 v2 = m_vertex2; + b2Vec2 e = v2 - v1; + b2Vec2 normal(e.y, -e.x); + normal.Normalize(); + + // q = p1 + t * d + // dot(normal, q - v1) = 0 + // dot(normal, p1 - v1) + t * dot(normal, d) = 0 + float32 numerator = b2Dot(normal, v1 - p1); + float32 denominator = b2Dot(normal, d); + + if (denominator == 0.0f) + { + return false; + } + + float32 t = numerator / denominator; + if (t < 0.0f || input.maxFraction < t) + { + return false; + } + + b2Vec2 q = p1 + t * d; + + // q = v1 + s * r + // s = dot(q - v1, r) / dot(r, r) + b2Vec2 r = v2 - v1; + float32 rr = b2Dot(r, r); + if (rr == 0.0f) + { + return false; + } + + float32 s = b2Dot(q - v1, r) / rr; + if (s < 0.0f || 1.0f < s) + { + return false; + } + + output->fraction = t; + if (numerator > 0.0f) + { + output->normal = -normal; + } + else + { + output->normal = normal; + } + return true; +} + +void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const +{ + B2_NOT_USED(childIndex); + + b2Vec2 v1 = b2Mul(xf, m_vertex1); + b2Vec2 v2 = b2Mul(xf, m_vertex2); + + b2Vec2 lower = b2Min(v1, v2); + b2Vec2 upper = b2Max(v1, v2); + + b2Vec2 r(m_radius, m_radius); + aabb->lowerBound = lower - r; + aabb->upperBound = upper + r; +} + +void b2EdgeShape::ComputeMass(b2MassData* massData, float32 density) const +{ + B2_NOT_USED(density); + + massData->mass = 0.0f; + massData->center = 0.5f * (m_vertex1 + m_vertex2); + massData->I = 0.0f; +} diff --git a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2EdgeShape.h b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2EdgeShape.h new file mode 100755 index 000000000..780eb2c5c --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2EdgeShape.h @@ -0,0 +1,70 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B2_EDGE_SHAPE_H +#define B2_EDGE_SHAPE_H + +#include + +/// A line segment (edge) shape. These can be connected in chains or loops +/// to other edge shapes. The connectivity information is used to ensure +/// correct contact normals. +class b2EdgeShape : public b2Shape +{ +public: + b2EdgeShape(); + + /// Set this as an isolated edge. + void Set(const b2Vec2& v1, const b2Vec2& v2); + + /// Implement b2Shape. + b2Shape* Clone(b2BlockAllocator* allocator) const; + + /// @see b2Shape::GetChildCount + int32 GetChildCount() const; + + /// @see b2Shape::TestPoint + bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; + + /// Implement b2Shape. + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const; + + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const; + + /// @see b2Shape::ComputeMass + void ComputeMass(b2MassData* massData, float32 density) const; + + /// These are the edge vertices + b2Vec2 m_vertex1, m_vertex2; + + /// Optional adjacent vertices. These are used for smooth collision. + b2Vec2 m_vertex0, m_vertex3; + bool m_hasVertex0, m_hasVertex3; +}; + +inline b2EdgeShape::b2EdgeShape() +{ + m_type = e_edge; + m_radius = b2_polygonRadius; + m_hasVertex0 = false; + m_hasVertex3 = false; +} + +#endif diff --git a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2PolygonShape.cpp b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2PolygonShape.cpp old mode 100644 new mode 100755 index 429e64704..8be813338 --- a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2PolygonShape.cpp +++ b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2PolygonShape.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -55,41 +55,29 @@ void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, floa m_centroid = center; b2Transform xf; - xf.position = center; - xf.R.Set(angle); + xf.p = center; + xf.q.Set(angle); // Transform vertices and normals. for (int32 i = 0; i < m_vertexCount; ++i) { m_vertices[i] = b2Mul(xf, m_vertices[i]); - m_normals[i] = b2Mul(xf.R, m_normals[i]); + m_normals[i] = b2Mul(xf.q, m_normals[i]); } } -void b2PolygonShape::SetAsEdge(const b2Vec2& v1, const b2Vec2& v2) +int32 b2PolygonShape::GetChildCount() const { - m_vertexCount = 2; - m_vertices[0] = v1; - m_vertices[1] = v2; - m_centroid = 0.5f * (v1 + v2); - m_normals[0] = b2Cross(v2 - v1, 1.0f); - m_normals[0].Normalize(); - m_normals[1] = -m_normals[0]; + return 1; } static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count) { - b2Assert(count >= 2); + b2Assert(count >= 3); b2Vec2 c; c.Set(0.0f, 0.0f); float32 area = 0.0f; - if (count == 2) - { - c = 0.5f * (vs[0] + vs[1]); - return c; - } - // pRef is the reference point for forming triangles. // It's location doesn't change the result (except for rounding error). b2Vec2 pRef(0.0f, 0.0f); @@ -131,7 +119,7 @@ static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count) void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) { - b2Assert(2 <= count && count <= b2_maxPolygonVertices); + b2Assert(3 <= count && count <= b2_maxPolygonVertices); m_vertexCount = count; // Copy vertices. @@ -170,10 +158,10 @@ void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) b2Vec2 r = m_vertices[j] - m_vertices[i1]; - // Your polygon is non-convex (it has an indentation) or - // has colinear edges. + // If this crashes, your polygon is non-convex, has colinear edges, + // or the winding order is wrong. float32 s = b2Cross(edge, r); - b2Assert(s > 0.0f); + b2Assert(s > 0.0f && "ERROR: Please ensure your polygon is convex and has a CCW winding order"); } } #endif @@ -184,7 +172,7 @@ void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const { - b2Vec2 pLocal = b2MulT(xf.R, p - xf.position); + b2Vec2 pLocal = b2MulT(xf.q, p - xf.p); for (int32 i = 0; i < m_vertexCount; ++i) { @@ -198,131 +186,82 @@ bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const return true; } -bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& xf) const +bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& xf, int32 childIndex) const { + B2_NOT_USED(childIndex); + // Put the ray into the polygon's frame of reference. - b2Vec2 p1 = b2MulT(xf.R, input.p1 - xf.position); - b2Vec2 p2 = b2MulT(xf.R, input.p2 - xf.position); + b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); + b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); b2Vec2 d = p2 - p1; - if (m_vertexCount == 2) - { - b2Vec2 v1 = m_vertices[0]; - b2Vec2 v2 = m_vertices[1]; - b2Vec2 normal = m_normals[0]; + float32 lower = 0.0f, upper = input.maxFraction; - // q = p1 + t * d - // dot(normal, q - v1) = 0 - // dot(normal, p1 - v1) + t * dot(normal, d) = 0 - float32 numerator = b2Dot(normal, v1 - p1); - float32 denominator = b2Dot(normal, d); + int32 index = -1; + + for (int32 i = 0; i < m_vertexCount; ++i) + { + // p = p1 + a * d + // dot(normal, p - v) = 0 + // dot(normal, p1 - v) + a * dot(normal, d) = 0 + float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1); + float32 denominator = b2Dot(m_normals[i], d); if (denominator == 0.0f) - { - return false; - } - - float32 t = numerator / denominator; - if (t < 0.0f || 1.0f < t) - { - return false; - } - - b2Vec2 q = p1 + t * d; - - // q = v1 + s * r - // s = dot(q - v1, r) / dot(r, r) - b2Vec2 r = v2 - v1; - float32 rr = b2Dot(r, r); - if (rr == 0.0f) - { - return false; - } - - float32 s = b2Dot(q - v1, r) / rr; - if (s < 0.0f || 1.0f < s) - { - return false; - } - - output->fraction = t; - if (numerator > 0.0f) - { - output->normal = -normal; - } - else - { - output->normal = normal; - } - return true; - } - else - { - float32 lower = 0.0f, upper = input.maxFraction; - - int32 index = -1; - - for (int32 i = 0; i < m_vertexCount; ++i) - { - // p = p1 + a * d - // dot(normal, p - v) = 0 - // dot(normal, p1 - v) + a * dot(normal, d) = 0 - float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1); - float32 denominator = b2Dot(m_normals[i], d); - - if (denominator == 0.0f) - { - if (numerator < 0.0f) - { - return false; - } - } - else - { - // Note: we want this predicate without division: - // lower < numerator / denominator, where denominator < 0 - // Since denominator < 0, we have to flip the inequality: - // lower < numerator / denominator <==> denominator * lower > numerator. - if (denominator < 0.0f && numerator < lower * denominator) - { - // Increase lower. - // The segment enters this half-space. - lower = numerator / denominator; - index = i; - } - else if (denominator > 0.0f && numerator < upper * denominator) - { - // Decrease upper. - // The segment exits this half-space. - upper = numerator / denominator; - } - } - - // The use of epsilon here causes the assert on lower to trip - // in some cases. Apparently the use of epsilon was to make edge - // shapes work, but now those are handled separately. - //if (upper < lower - b2_epsilon) - if (upper < lower) + { + if (numerator < 0.0f) { return false; } } - - b2Assert(0.0f <= lower && lower <= input.maxFraction); - - if (index >= 0) + else { - output->fraction = lower; - output->normal = b2Mul(xf.R, m_normals[index]); - return true; + // Note: we want this predicate without division: + // lower < numerator / denominator, where denominator < 0 + // Since denominator < 0, we have to flip the inequality: + // lower < numerator / denominator <==> denominator * lower > numerator. + if (denominator < 0.0f && numerator < lower * denominator) + { + // Increase lower. + // The segment enters this half-space. + lower = numerator / denominator; + index = i; + } + else if (denominator > 0.0f && numerator < upper * denominator) + { + // Decrease upper. + // The segment exits this half-space. + upper = numerator / denominator; + } } + + // The use of epsilon here causes the assert on lower to trip + // in some cases. Apparently the use of epsilon was to make edge + // shapes work, but now those are handled separately. + //if (upper < lower - b2_epsilon) + if (upper < lower) + { + return false; + } + } + + b2Assert(0.0f <= lower && lower <= input.maxFraction); + + if (index >= 0) + { + output->fraction = lower; + output->normal = b2Mul(xf.q, m_normals[index]); + return true; } return false; } -void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf) const +void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const { + B2_NOT_USED(childIndex); + b2Vec2 lower = b2Mul(xf, m_vertices[0]); b2Vec2 upper = lower; @@ -364,44 +303,30 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const // // The rest of the derivation is handled by computer algebra. - b2Assert(m_vertexCount >= 2); - - // A line segment has zero mass. - if (m_vertexCount == 2) - { - massData->center = 0.5f * (m_vertices[0] + m_vertices[1]); - massData->mass = 0.0f; - massData->I = 0.0f; - return; - } + b2Assert(m_vertexCount >= 3); b2Vec2 center; center.Set(0.0f, 0.0f); float32 area = 0.0f; float32 I = 0.0f; - // pRef is the reference point for forming triangles. + // s is the reference point for forming triangles. // It's location doesn't change the result (except for rounding error). - b2Vec2 pRef(0.0f, 0.0f); -#if 0 + b2Vec2 s(0.0f, 0.0f); + // This code would put the reference point inside the polygon. for (int32 i = 0; i < m_vertexCount; ++i) { - pRef += m_vertices[i]; + s += m_vertices[i]; } - pRef *= 1.0f / count; -#endif + s *= 1.0f / m_vertexCount; const float32 k_inv3 = 1.0f / 3.0f; for (int32 i = 0; i < m_vertexCount; ++i) { // Triangle vertices. - b2Vec2 p1 = pRef; - b2Vec2 p2 = m_vertices[i]; - b2Vec2 p3 = i + 1 < m_vertexCount ? m_vertices[i+1] : m_vertices[0]; - - b2Vec2 e1 = p2 - p1; - b2Vec2 e2 = p3 - p1; + b2Vec2 e1 = m_vertices[i] - s; + b2Vec2 e2 = i + 1 < m_vertexCount ? m_vertices[i+1] - s : m_vertices[0] - s; float32 D = b2Cross(e1, e2); @@ -409,16 +334,15 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const area += triangleArea; // Area weighted centroid - center += triangleArea * k_inv3 * (p1 + p2 + p3); + center += triangleArea * k_inv3 * (e1 + e2); - float32 px = p1.x, py = p1.y; float32 ex1 = e1.x, ey1 = e1.y; float32 ex2 = e2.x, ey2 = e2.y; - float32 intx2 = k_inv3 * (0.25f * (ex1*ex1 + ex2*ex1 + ex2*ex2) + (px*ex1 + px*ex2)) + 0.5f*px*px; - float32 inty2 = k_inv3 * (0.25f * (ey1*ey1 + ey2*ey1 + ey2*ey2) + (py*ey1 + py*ey2)) + 0.5f*py*py; + float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2; + float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2; - I += D * (intx2 + inty2); + I += (0.25f * k_inv3 * D) * (intx2 + inty2); } // Total mass @@ -427,8 +351,11 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const // Center of mass b2Assert(area > b2_epsilon); center *= 1.0f / area; - massData->center = center; + massData->center = center + s; - // Inertia tensor relative to the local origin. + // Inertia tensor relative to the local origin (point s). massData->I = density * I; + + // Shift to center of mass then to original body origin. + massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center)); } diff --git a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2PolygonShape.h b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2PolygonShape.h old mode 100644 new mode 100755 index 564d4b083..fd11bd162 --- a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2PolygonShape.h +++ b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2PolygonShape.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -23,6 +23,8 @@ /// A convex polygon. It is assumed that the interior of the polygon is to /// the left of each edge. +/// Polygons have a maximum number of vertices equal to b2_maxPolygonVertices. +/// In most cases you should not need many vertices for a convex polygon. class b2PolygonShape : public b2Shape { public: @@ -31,8 +33,12 @@ public: /// Implement b2Shape. b2Shape* Clone(b2BlockAllocator* allocator) const; + /// @see b2Shape::GetChildCount + int32 GetChildCount() const; + /// Copy vertices. This assumes the vertices define a convex polygon. /// It is assumed that the exterior is the the right of each edge. + /// The count must be in the range [3, b2_maxPolygonVertices]. void Set(const b2Vec2* vertices, int32 vertexCount); /// Build vertices to represent an axis-aligned box. @@ -47,27 +53,19 @@ public: /// @param angle the rotation of the box in local coordinates. void SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle); - /// Set this as a single edge. - void SetAsEdge(const b2Vec2& v1, const b2Vec2& v2); - /// @see b2Shape::TestPoint bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; /// Implement b2Shape. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const; + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const; /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform) const; + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const; /// @see b2Shape::ComputeMass void ComputeMass(b2MassData* massData, float32 density) const; - /// Get the supporting vertex index in the given direction. - int32 GetSupport(const b2Vec2& d) const; - - /// Get the supporting vertex in the given direction. - const b2Vec2& GetSupportVertex(const b2Vec2& d) const; - /// Get the vertex count. int32 GetVertexCount() const { return m_vertexCount; } @@ -88,40 +86,6 @@ inline b2PolygonShape::b2PolygonShape() m_centroid.SetZero(); } -inline int32 b2PolygonShape::GetSupport(const b2Vec2& d) const -{ - int32 bestIndex = 0; - float32 bestValue = b2Dot(m_vertices[0], d); - for (int32 i = 1; i < m_vertexCount; ++i) - { - float32 value = b2Dot(m_vertices[i], d); - if (value > bestValue) - { - bestIndex = i; - bestValue = value; - } - } - - return bestIndex; -} - -inline const b2Vec2& b2PolygonShape::GetSupportVertex(const b2Vec2& d) const -{ - int32 bestIndex = 0; - float32 bestValue = b2Dot(m_vertices[0], d); - for (int32 i = 1; i < m_vertexCount; ++i) - { - float32 value = b2Dot(m_vertices[i], d); - if (value > bestValue) - { - bestIndex = i; - bestValue = value; - } - } - - return m_vertices[bestIndex]; -} - inline const b2Vec2& b2PolygonShape::GetVertex(int32 index) const { b2Assert(0 <= index && index < m_vertexCount); diff --git a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2Shape.h b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2Shape.h old mode 100644 new mode 100755 index 9082c0ef9..fd7de2626 --- a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2Shape.h +++ b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2Shape.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -38,20 +38,20 @@ struct b2MassData /// A shape is used for collision detection. You can create a shape however you like. /// Shapes used for simulation in b2World are created automatically when a b2Fixture -/// is created. +/// is created. Shapes may encapsulate a one or more child shapes. class b2Shape { public: enum Type { - e_unknown= -1, e_circle = 0, - e_polygon = 1, - e_typeCount = 2, + e_edge = 1, + e_polygon = 2, + e_chain = 3, + e_typeCount = 4 }; - b2Shape() { m_type = e_unknown; } virtual ~b2Shape() {} /// Clone the concrete shape using the provided allocator. @@ -61,21 +61,27 @@ public: /// @return the shape type. Type GetType() const; + /// Get the number of child primitives. + virtual int32 GetChildCount() const = 0; + /// Test a point for containment in this shape. This only works for convex shapes. /// @param xf the shape world transform. /// @param p a point in world coordinates. virtual bool TestPoint(const b2Transform& xf, const b2Vec2& p) const = 0; - /// Cast a ray against this shape. + /// Cast a ray against a child shape. /// @param output the ray-cast results. /// @param input the ray-cast input parameters. /// @param transform the transform to be applied to the shape. - virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const = 0; + /// @param childIndex the child shape index + virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const = 0; - /// Given a transform, compute the associated axis aligned bounding box for this shape. + /// Given a transform, compute the associated axis aligned bounding box for a child shape. /// @param aabb returns the axis aligned box. /// @param xf the world transform of the shape. - virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf) const = 0; + /// @param childIndex the child shape + virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const = 0; /// Compute the mass properties of this shape using its dimensions and density. /// The inertia tensor is computed about the local origin. diff --git a/src/modules/physics/box2d/Box2D/Collision/b2BroadPhase.cpp b/src/modules/physics/box2d/Box2D/Collision/b2BroadPhase.cpp old mode 100644 new mode 100755 index 12c79678d..2aa62f948 --- a/src/modules/physics/box2d/Box2D/Collision/b2BroadPhase.cpp +++ b/src/modules/physics/box2d/Box2D/Collision/b2BroadPhase.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -18,6 +18,7 @@ #include #include +using namespace std; b2BroadPhase::b2BroadPhase() { @@ -62,6 +63,11 @@ void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& di } } +void b2BroadPhase::TouchProxy(int32 proxyId) +{ + BufferMove(proxyId); +} + void b2BroadPhase::BufferMove(int32 proxyId) { if (m_moveCount == m_moveCapacity) diff --git a/src/modules/physics/box2d/Box2D/Collision/b2BroadPhase.h b/src/modules/physics/box2d/Box2D/Collision/b2BroadPhase.h old mode 100644 new mode 100755 index bff188e88..c7398c97a --- a/src/modules/physics/box2d/Box2D/Collision/b2BroadPhase.h +++ b/src/modules/physics/box2d/Box2D/Collision/b2BroadPhase.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -40,7 +40,7 @@ public: enum { - e_nullProxy = -1, + e_nullProxy = -1 }; b2BroadPhase(); @@ -57,6 +57,9 @@ public: /// call UpdatePairs to finalized the proxy pairs (for your time step). void MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement); + /// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs. + void TouchProxy(int32 proxyId); + /// Get the fat AABB for a proxy. const b2AABB& GetFatAABB(int32 proxyId) const; @@ -88,8 +91,14 @@ public: template void RayCast(T* callback, const b2RayCastInput& input) const; - /// Compute the height of the embedded tree. - int32 ComputeHeight() const; + /// Get the height of the embedded tree. + int32 GetTreeHeight() const; + + /// Get the balance of the embedded tree. + int32 GetTreeBalance() const; + + /// Get the quality metric of the embedded tree. + float32 GetTreeQuality() const; private: @@ -153,9 +162,19 @@ inline int32 b2BroadPhase::GetProxyCount() const return m_proxyCount; } -inline int32 b2BroadPhase::ComputeHeight() const +inline int32 b2BroadPhase::GetTreeHeight() const { - return m_tree.ComputeHeight(); + return m_tree.GetHeight(); +} + +inline int32 b2BroadPhase::GetTreeBalance() const +{ + return m_tree.GetMaxBalance(); +} + +inline float32 b2BroadPhase::GetTreeQuality() const +{ + return m_tree.GetAreaRatio(); } template @@ -211,7 +230,7 @@ void b2BroadPhase::UpdatePairs(T* callback) } // Try to keep the tree balanced. - m_tree.Rebalance(4); + //m_tree.Rebalance(4); } template diff --git a/src/modules/physics/box2d/Box2D/Collision/b2CollideCircle.cpp b/src/modules/physics/box2d/Box2D/Collision/b2CollideCircle.cpp old mode 100644 new mode 100755 index 6edf89d7b..0ad58f000 --- a/src/modules/physics/box2d/Box2D/Collision/b2CollideCircle.cpp +++ b/src/modules/physics/box2d/Box2D/Collision/b2CollideCircle.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages diff --git a/src/modules/physics/box2d/Box2D/Collision/b2CollideEdge.cpp b/src/modules/physics/box2d/Box2D/Collision/b2CollideEdge.cpp new file mode 100755 index 000000000..281967183 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Collision/b2CollideEdge.cpp @@ -0,0 +1,698 @@ +/* + * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +#include +#include +#include +#include + + +// Compute contact points for edge versus circle. +// This accounts for edge connectivity. +void b2CollideEdgeAndCircle(b2Manifold* manifold, + const b2EdgeShape* edgeA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB) +{ + manifold->pointCount = 0; + + // Compute circle in frame of edge + b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p)); + + b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2; + b2Vec2 e = B - A; + + // Barycentric coordinates + float32 u = b2Dot(e, B - Q); + float32 v = b2Dot(e, Q - A); + + float32 radius = edgeA->m_radius + circleB->m_radius; + + b2ContactFeature cf; + cf.indexB = 0; + cf.typeB = b2ContactFeature::e_vertex; + + // Region A + if (v <= 0.0f) + { + b2Vec2 P = A; + b2Vec2 d = Q - P; + float32 dd = b2Dot(d, d); + if (dd > radius * radius) + { + return; + } + + // Is there an edge connected to A? + if (edgeA->m_hasVertex0) + { + b2Vec2 A1 = edgeA->m_vertex0; + b2Vec2 B1 = A; + b2Vec2 e1 = B1 - A1; + float32 u1 = b2Dot(e1, B1 - Q); + + // Is the circle in Region AB of the previous edge? + if (u1 > 0.0f) + { + return; + } + } + + cf.indexA = 0; + cf.typeA = b2ContactFeature::e_vertex; + manifold->pointCount = 1; + manifold->type = b2Manifold::e_circles; + manifold->localNormal.SetZero(); + manifold->localPoint = P; + manifold->points[0].id.key = 0; + manifold->points[0].id.cf = cf; + manifold->points[0].localPoint = circleB->m_p; + return; + } + + // Region B + if (u <= 0.0f) + { + b2Vec2 P = B; + b2Vec2 d = Q - P; + float32 dd = b2Dot(d, d); + if (dd > radius * radius) + { + return; + } + + // Is there an edge connected to B? + if (edgeA->m_hasVertex3) + { + b2Vec2 B2 = edgeA->m_vertex3; + b2Vec2 A2 = B; + b2Vec2 e2 = B2 - A2; + float32 v2 = b2Dot(e2, Q - A2); + + // Is the circle in Region AB of the next edge? + if (v2 > 0.0f) + { + return; + } + } + + cf.indexA = 1; + cf.typeA = b2ContactFeature::e_vertex; + manifold->pointCount = 1; + manifold->type = b2Manifold::e_circles; + manifold->localNormal.SetZero(); + manifold->localPoint = P; + manifold->points[0].id.key = 0; + manifold->points[0].id.cf = cf; + manifold->points[0].localPoint = circleB->m_p; + return; + } + + // Region AB + float32 den = b2Dot(e, e); + b2Assert(den > 0.0f); + b2Vec2 P = (1.0f / den) * (u * A + v * B); + b2Vec2 d = Q - P; + float32 dd = b2Dot(d, d); + if (dd > radius * radius) + { + return; + } + + b2Vec2 n(-e.y, e.x); + if (b2Dot(n, Q - A) < 0.0f) + { + n.Set(-n.x, -n.y); + } + n.Normalize(); + + cf.indexA = 0; + cf.typeA = b2ContactFeature::e_face; + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = n; + manifold->localPoint = A; + manifold->points[0].id.key = 0; + manifold->points[0].id.cf = cf; + manifold->points[0].localPoint = circleB->m_p; +} + +// This structure is used to keep track of the best separating axis. +struct b2EPAxis +{ + enum Type + { + e_unknown, + e_edgeA, + e_edgeB + }; + + Type type; + int32 index; + float32 separation; +}; + +// This holds polygon B expressed in frame A. +struct b2TempPolygon +{ + b2Vec2 vertices[b2_maxPolygonVertices]; + b2Vec2 normals[b2_maxPolygonVertices]; + int32 count; +}; + +// Reference face used for clipping +struct b2ReferenceFace +{ + int32 i1, i2; + + b2Vec2 v1, v2; + + b2Vec2 normal; + + b2Vec2 sideNormal1; + float32 sideOffset1; + + b2Vec2 sideNormal2; + float32 sideOffset2; +}; + +// This class collides and edge and a polygon, taking into account edge adjacency. +struct b2EPCollider +{ + void Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, + const b2PolygonShape* polygonB, const b2Transform& xfB); + b2EPAxis ComputeEdgeSeparation(); + b2EPAxis ComputePolygonSeparation(); + + enum VertexType + { + e_isolated, + e_concave, + e_convex + }; + + b2TempPolygon m_polygonB; + + b2Transform m_xf; + b2Vec2 m_centroidB; + b2Vec2 m_v0, m_v1, m_v2, m_v3; + b2Vec2 m_normal0, m_normal1, m_normal2; + b2Vec2 m_normal; + VertexType m_type1, m_type2; + b2Vec2 m_lowerLimit, m_upperLimit; + float32 m_radius; + bool m_front; +}; + +// Algorithm: +// 1. Classify v1 and v2 +// 2. Classify polygon centroid as front or back +// 3. Flip normal if necessary +// 4. Initialize normal range to [-pi, pi] about face normal +// 5. Adjust normal range according to adjacent edges +// 6. Visit each separating axes, only accept axes within the range +// 7. Return if _any_ axis indicates separation +// 8. Clip +void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, + const b2PolygonShape* polygonB, const b2Transform& xfB) +{ + m_xf = b2MulT(xfA, xfB); + + m_centroidB = b2Mul(m_xf, polygonB->m_centroid); + + m_v0 = edgeA->m_vertex0; + m_v1 = edgeA->m_vertex1; + m_v2 = edgeA->m_vertex2; + m_v3 = edgeA->m_vertex3; + + bool hasVertex0 = edgeA->m_hasVertex0; + bool hasVertex3 = edgeA->m_hasVertex3; + + b2Vec2 edge1 = m_v2 - m_v1; + edge1.Normalize(); + m_normal1.Set(edge1.y, -edge1.x); + float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1); + float32 offset0 = 0.0f, offset2 = 0.0f; + bool convex1 = false, convex2 = false; + + // Is there a preceding edge? + if (hasVertex0) + { + b2Vec2 edge0 = m_v1 - m_v0; + edge0.Normalize(); + m_normal0.Set(edge0.y, -edge0.x); + convex1 = b2Cross(edge0, edge1) >= 0.0f; + offset0 = b2Dot(m_normal0, m_centroidB - m_v0); + } + + // Is there a following edge? + if (hasVertex3) + { + b2Vec2 edge2 = m_v3 - m_v2; + edge2.Normalize(); + m_normal2.Set(edge2.y, -edge2.x); + convex2 = b2Cross(edge1, edge2) > 0.0f; + offset2 = b2Dot(m_normal2, m_centroidB - m_v2); + } + + // Determine front or back collision. Determine collision normal limits. + if (hasVertex0 && hasVertex3) + { + if (convex1 && convex2) + { + m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f; + if (m_front) + { + m_normal = m_normal1; + m_lowerLimit = m_normal0; + m_upperLimit = m_normal2; + } + else + { + m_normal = -m_normal1; + m_lowerLimit = -m_normal1; + m_upperLimit = -m_normal1; + } + } + else if (convex1) + { + m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); + if (m_front) + { + m_normal = m_normal1; + m_lowerLimit = m_normal0; + m_upperLimit = m_normal1; + } + else + { + m_normal = -m_normal1; + m_lowerLimit = -m_normal2; + m_upperLimit = -m_normal1; + } + } + else if (convex2) + { + m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); + if (m_front) + { + m_normal = m_normal1; + m_lowerLimit = m_normal1; + m_upperLimit = m_normal2; + } + else + { + m_normal = -m_normal1; + m_lowerLimit = -m_normal1; + m_upperLimit = -m_normal0; + } + } + else + { + m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; + if (m_front) + { + m_normal = m_normal1; + m_lowerLimit = m_normal1; + m_upperLimit = m_normal1; + } + else + { + m_normal = -m_normal1; + m_lowerLimit = -m_normal2; + m_upperLimit = -m_normal0; + } + } + } + else if (hasVertex0) + { + if (convex1) + { + m_front = offset0 >= 0.0f || offset1 >= 0.0f; + if (m_front) + { + m_normal = m_normal1; + m_lowerLimit = m_normal0; + m_upperLimit = -m_normal1; + } + else + { + m_normal = -m_normal1; + m_lowerLimit = m_normal1; + m_upperLimit = -m_normal1; + } + } + else + { + m_front = offset0 >= 0.0f && offset1 >= 0.0f; + if (m_front) + { + m_normal = m_normal1; + m_lowerLimit = m_normal1; + m_upperLimit = -m_normal1; + } + else + { + m_normal = -m_normal1; + m_lowerLimit = m_normal1; + m_upperLimit = -m_normal0; + } + } + } + else if (hasVertex3) + { + if (convex2) + { + m_front = offset1 >= 0.0f || offset2 >= 0.0f; + if (m_front) + { + m_normal = m_normal1; + m_lowerLimit = -m_normal1; + m_upperLimit = m_normal2; + } + else + { + m_normal = -m_normal1; + m_lowerLimit = -m_normal1; + m_upperLimit = m_normal1; + } + } + else + { + m_front = offset1 >= 0.0f && offset2 >= 0.0f; + if (m_front) + { + m_normal = m_normal1; + m_lowerLimit = -m_normal1; + m_upperLimit = m_normal1; + } + else + { + m_normal = -m_normal1; + m_lowerLimit = -m_normal2; + m_upperLimit = m_normal1; + } + } + } + else + { + m_front = offset1 >= 0.0f; + if (m_front) + { + m_normal = m_normal1; + m_lowerLimit = -m_normal1; + m_upperLimit = -m_normal1; + } + else + { + m_normal = -m_normal1; + m_lowerLimit = m_normal1; + m_upperLimit = m_normal1; + } + } + + // Get polygonB in frameA + m_polygonB.count = polygonB->m_vertexCount; + for (int32 i = 0; i < polygonB->m_vertexCount; ++i) + { + m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]); + m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]); + } + + m_radius = 2.0f * b2_polygonRadius; + + manifold->pointCount = 0; + + b2EPAxis edgeAxis = ComputeEdgeSeparation(); + + // If no valid normal can be found than this edge should not collide. + if (edgeAxis.type == b2EPAxis::e_unknown) + { + return; + } + + if (edgeAxis.separation > m_radius) + { + return; + } + + b2EPAxis polygonAxis = ComputePolygonSeparation(); + if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius) + { + return; + } + + // Use hysteresis for jitter reduction. + const float32 k_relativeTol = 0.98f; + const float32 k_absoluteTol = 0.001f; + + b2EPAxis primaryAxis; + if (polygonAxis.type == b2EPAxis::e_unknown) + { + primaryAxis = edgeAxis; + } + else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) + { + primaryAxis = polygonAxis; + } + else + { + primaryAxis = edgeAxis; + } + + b2ClipVertex ie[2]; + b2ReferenceFace rf; + if (primaryAxis.type == b2EPAxis::e_edgeA) + { + manifold->type = b2Manifold::e_faceA; + + // Search for the polygon normal that is most anti-parallel to the edge normal. + int32 bestIndex = 0; + float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]); + for (int32 i = 1; i < m_polygonB.count; ++i) + { + float32 value = b2Dot(m_normal, m_polygonB.normals[i]); + if (value < bestValue) + { + bestValue = value; + bestIndex = i; + } + } + + int32 i1 = bestIndex; + int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0; + + ie[0].v = m_polygonB.vertices[i1]; + ie[0].id.cf.indexA = 0; + ie[0].id.cf.indexB = i1; + ie[0].id.cf.typeA = b2ContactFeature::e_face; + ie[0].id.cf.typeB = b2ContactFeature::e_vertex; + + ie[1].v = m_polygonB.vertices[i2]; + ie[1].id.cf.indexA = 0; + ie[1].id.cf.indexB = i2; + ie[1].id.cf.typeA = b2ContactFeature::e_face; + ie[1].id.cf.typeB = b2ContactFeature::e_vertex; + + if (m_front) + { + rf.i1 = 0; + rf.i2 = 1; + rf.v1 = m_v1; + rf.v2 = m_v2; + rf.normal = m_normal1; + } + else + { + rf.i1 = 1; + rf.i2 = 0; + rf.v1 = m_v2; + rf.v2 = m_v1; + rf.normal = -m_normal1; + } + } + else + { + manifold->type = b2Manifold::e_faceB; + + ie[0].v = m_v1; + ie[0].id.cf.indexA = 0; + ie[0].id.cf.indexB = primaryAxis.index; + ie[0].id.cf.typeA = b2ContactFeature::e_vertex; + ie[0].id.cf.typeB = b2ContactFeature::e_face; + + ie[1].v = m_v2; + ie[1].id.cf.indexA = 0; + ie[1].id.cf.indexB = primaryAxis.index; + ie[1].id.cf.typeA = b2ContactFeature::e_vertex; + ie[1].id.cf.typeB = b2ContactFeature::e_face; + + rf.i1 = primaryAxis.index; + rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0; + rf.v1 = m_polygonB.vertices[rf.i1]; + rf.v2 = m_polygonB.vertices[rf.i2]; + rf.normal = m_polygonB.normals[rf.i1]; + } + + rf.sideNormal1.Set(rf.normal.y, -rf.normal.x); + rf.sideNormal2 = -rf.sideNormal1; + rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1); + rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2); + + // Clip incident edge against extruded edge1 side edges. + b2ClipVertex clipPoints1[2]; + b2ClipVertex clipPoints2[2]; + int32 np; + + // Clip to box side 1 + np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1); + + if (np < b2_maxManifoldPoints) + { + return; + } + + // Clip to negative box side 1 + np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2); + + if (np < b2_maxManifoldPoints) + { + return; + } + + // Now clipPoints2 contains the clipped points. + if (primaryAxis.type == b2EPAxis::e_edgeA) + { + manifold->localNormal = rf.normal; + manifold->localPoint = rf.v1; + } + else + { + manifold->localNormal = polygonB->m_normals[rf.i1]; + manifold->localPoint = polygonB->m_vertices[rf.i1]; + } + + int32 pointCount = 0; + for (int32 i = 0; i < b2_maxManifoldPoints; ++i) + { + float32 separation; + + separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1); + + if (separation <= m_radius) + { + b2ManifoldPoint* cp = manifold->points + pointCount; + + if (primaryAxis.type == b2EPAxis::e_edgeA) + { + cp->localPoint = b2MulT(m_xf, clipPoints2[i].v); + cp->id = clipPoints2[i].id; + } + else + { + cp->localPoint = clipPoints2[i].v; + cp->id.cf.typeA = clipPoints2[i].id.cf.typeB; + cp->id.cf.typeB = clipPoints2[i].id.cf.typeA; + cp->id.cf.indexA = clipPoints2[i].id.cf.indexB; + cp->id.cf.indexB = clipPoints2[i].id.cf.indexA; + } + + ++pointCount; + } + } + + manifold->pointCount = pointCount; +} + +b2EPAxis b2EPCollider::ComputeEdgeSeparation() +{ + b2EPAxis axis; + axis.type = b2EPAxis::e_edgeA; + axis.index = m_front ? 0 : 1; + axis.separation = FLT_MAX; + + for (int32 i = 0; i < m_polygonB.count; ++i) + { + float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1); + if (s < axis.separation) + { + axis.separation = s; + } + } + + return axis; +} + +b2EPAxis b2EPCollider::ComputePolygonSeparation() +{ + b2EPAxis axis; + axis.type = b2EPAxis::e_unknown; + axis.index = -1; + axis.separation = -FLT_MAX; + + b2Vec2 perp(-m_normal.y, m_normal.x); + + for (int32 i = 0; i < m_polygonB.count; ++i) + { + b2Vec2 n = -m_polygonB.normals[i]; + + float32 s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1); + float32 s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2); + float32 s = b2Min(s1, s2); + + if (s > m_radius) + { + // No collision + axis.type = b2EPAxis::e_edgeB; + axis.index = i; + axis.separation = s; + return axis; + } + + // Adjacency + if (b2Dot(n, perp) >= 0.0f) + { + if (b2Dot(n - m_upperLimit, m_normal) < -b2_angularSlop) + { + continue; + } + } + else + { + if (b2Dot(n - m_lowerLimit, m_normal) < -b2_angularSlop) + { + continue; + } + } + + if (s > axis.separation) + { + axis.type = b2EPAxis::e_edgeB; + axis.index = i; + axis.separation = s; + } + } + + return axis; +} + +void b2CollideEdgeAndPolygon( b2Manifold* manifold, + const b2EdgeShape* edgeA, const b2Transform& xfA, + const b2PolygonShape* polygonB, const b2Transform& xfB) +{ + b2EPCollider collider; + collider.Collide(manifold, edgeA, xfA, polygonB, xfB); +} diff --git a/src/modules/physics/box2d/Box2D/Collision/b2CollidePolygon.cpp b/src/modules/physics/box2d/Box2D/Collision/b2CollidePolygon.cpp old mode 100644 new mode 100755 index b37b7ba92..2f0946b92 --- a/src/modules/physics/box2d/Box2D/Collision/b2CollidePolygon.cpp +++ b/src/modules/physics/box2d/Box2D/Collision/b2CollidePolygon.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -23,18 +23,17 @@ static float32 b2EdgeSeparation(const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1, const b2PolygonShape* poly2, const b2Transform& xf2) { - int32 count1 = poly1->m_vertexCount; const b2Vec2* vertices1 = poly1->m_vertices; const b2Vec2* normals1 = poly1->m_normals; int32 count2 = poly2->m_vertexCount; const b2Vec2* vertices2 = poly2->m_vertices; - b2Assert(0 <= edge1 && edge1 < count1); + b2Assert(0 <= edge1 && edge1 < poly1->m_vertexCount); // Convert normal from poly1's frame into poly2's frame. - b2Vec2 normal1World = b2Mul(xf1.R, normals1[edge1]); - b2Vec2 normal1 = b2MulT(xf2.R, normal1World); + b2Vec2 normal1World = b2Mul(xf1.q, normals1[edge1]); + b2Vec2 normal1 = b2MulT(xf2.q, normal1World); // Find support vertex on poly2 for -normal. int32 index = 0; @@ -66,7 +65,7 @@ static float32 b2FindMaxSeparation(int32* edgeIndex, // Vector pointing from the centroid of poly1 to the centroid of poly2. b2Vec2 d = b2Mul(xf2, poly2->m_centroid) - b2Mul(xf1, poly1->m_centroid); - b2Vec2 dLocal1 = b2MulT(xf1.R, d); + b2Vec2 dLocal1 = b2MulT(xf1.q, d); // Find edge normal on poly1 that has the largest projection onto d. int32 edge = 0; @@ -143,17 +142,16 @@ static void b2FindIncidentEdge(b2ClipVertex c[2], const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1, const b2PolygonShape* poly2, const b2Transform& xf2) { - int32 count1 = poly1->m_vertexCount; const b2Vec2* normals1 = poly1->m_normals; int32 count2 = poly2->m_vertexCount; const b2Vec2* vertices2 = poly2->m_vertices; const b2Vec2* normals2 = poly2->m_normals; - b2Assert(0 <= edge1 && edge1 < count1); + b2Assert(0 <= edge1 && edge1 < poly1->m_vertexCount); // Get the normal of the reference edge in poly2's frame. - b2Vec2 normal1 = b2MulT(xf2.R, b2Mul(xf1.R, normals1[edge1])); + b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1])); // Find the incident edge on poly2. int32 index = 0; @@ -173,14 +171,16 @@ static void b2FindIncidentEdge(b2ClipVertex c[2], int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0; c[0].v = b2Mul(xf2, vertices2[i1]); - c[0].id.features.referenceEdge = (uint8)edge1; - c[0].id.features.incidentEdge = (uint8)i1; - c[0].id.features.incidentVertex = 0; + c[0].id.cf.indexA = (uint8)edge1; + c[0].id.cf.indexB = (uint8)i1; + c[0].id.cf.typeA = b2ContactFeature::e_face; + c[0].id.cf.typeB = b2ContactFeature::e_vertex; c[1].v = b2Mul(xf2, vertices2[i2]); - c[1].id.features.referenceEdge = (uint8)edge1; - c[1].id.features.incidentEdge = (uint8)i2; - c[1].id.features.incidentVertex = 1; + c[1].id.cf.indexA = (uint8)edge1; + c[1].id.cf.indexB = (uint8)i2; + c[1].id.cf.typeA = b2ContactFeature::e_face; + c[1].id.cf.typeB = b2ContactFeature::e_vertex; } // Find edge normal of max separation on A - return if separating axis is found @@ -242,8 +242,11 @@ void b2CollidePolygons(b2Manifold* manifold, int32 count1 = poly1->m_vertexCount; const b2Vec2* vertices1 = poly1->m_vertices; - b2Vec2 v11 = vertices1[edge1]; - b2Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0]; + int32 iv1 = edge1; + int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; + + b2Vec2 v11 = vertices1[iv1]; + b2Vec2 v12 = vertices1[iv2]; b2Vec2 localTangent = v12 - v11; localTangent.Normalize(); @@ -251,7 +254,7 @@ void b2CollidePolygons(b2Manifold* manifold, b2Vec2 localNormal = b2Cross(localTangent, 1.0f); b2Vec2 planePoint = 0.5f * (v11 + v12); - b2Vec2 tangent = b2Mul(xf1.R, localTangent); + b2Vec2 tangent = b2Mul(xf1.q, localTangent); b2Vec2 normal = b2Cross(tangent, 1.0f); v11 = b2Mul(xf1, v11); @@ -270,13 +273,13 @@ void b2CollidePolygons(b2Manifold* manifold, int np; // Clip to box side 1 - np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1); + np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1); if (np < 2) return; // Clip to negative box side 1 - np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2); + np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2); if (np < 2) { @@ -297,7 +300,15 @@ void b2CollidePolygons(b2Manifold* manifold, b2ManifoldPoint* cp = manifold->points + pointCount; cp->localPoint = b2MulT(xf2, clipPoints2[i].v); cp->id = clipPoints2[i].id; - cp->id.features.flip = flip; + if (flip) + { + // Swap features + b2ContactFeature cf = cp->id.cf; + cp->id.cf.indexA = cf.indexB; + cp->id.cf.indexB = cf.indexA; + cp->id.cf.typeA = cf.typeB; + cp->id.cf.typeB = cf.typeA; + } ++pointCount; } } diff --git a/src/modules/physics/box2d/Box2D/Collision/b2Collision.cpp b/src/modules/physics/box2d/Box2D/Collision/b2Collision.cpp old mode 100644 new mode 100755 index a86c7c5a8..4b092f0cb --- a/src/modules/physics/box2d/Box2D/Collision/b2Collision.cpp +++ b/src/modules/physics/box2d/Box2D/Collision/b2Collision.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -49,7 +49,7 @@ void b2WorldManifold::Initialize(const b2Manifold* manifold, case b2Manifold::e_faceA: { - normal = b2Mul(xfA.R, manifold->localNormal); + normal = b2Mul(xfA.q, manifold->localNormal); b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint); for (int32 i = 0; i < manifold->pointCount; ++i) @@ -64,7 +64,7 @@ void b2WorldManifold::Initialize(const b2Manifold* manifold, case b2Manifold::e_faceB: { - normal = b2Mul(xfB.R, manifold->localNormal); + normal = b2Mul(xfB.q, manifold->localNormal); b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint); for (int32 i = 0; i < manifold->pointCount; ++i) @@ -196,7 +196,7 @@ bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const // Sutherland-Hodgman clipping. int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], - const b2Vec2& normal, float32 offset) + const b2Vec2& normal, float32 offset, int32 vertexIndexA) { // Start with no output points int32 numOut = 0; @@ -215,26 +215,25 @@ int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], // Find intersection point of edge and plane float32 interp = distance0 / (distance0 - distance1); vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); - if (distance0 > 0.0f) - { - vOut[numOut].id = vIn[0].id; - } - else - { - vOut[numOut].id = vIn[1].id; - } + + // VertexA is hitting edgeB. + vOut[numOut].id.cf.indexA = vertexIndexA; + vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB; + vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex; + vOut[numOut].id.cf.typeB = b2ContactFeature::e_face; ++numOut; } return numOut; } -bool b2TestOverlap(const b2Shape* shapeA, const b2Shape* shapeB, - const b2Transform& xfA, const b2Transform& xfB) +bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, + const b2Shape* shapeB, int32 indexB, + const b2Transform& xfA, const b2Transform& xfB) { b2DistanceInput input; - input.proxyA.Set(shapeA); - input.proxyB.Set(shapeB); + input.proxyA.Set(shapeA, indexA); + input.proxyB.Set(shapeB, indexB); input.transformA = xfA; input.transformB = xfB; input.useRadii = true; diff --git a/src/modules/physics/box2d/Box2D/Collision/b2Collision.h b/src/modules/physics/box2d/Box2D/Collision/b2Collision.h old mode 100644 new mode 100755 index baffdbde0..27897f70a --- a/src/modules/physics/box2d/Box2D/Collision/b2Collision.h +++ b/src/modules/physics/box2d/Box2D/Collision/b2Collision.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -28,21 +28,31 @@ class b2Shape; class b2CircleShape; +class b2EdgeShape; class b2PolygonShape; const uint8 b2_nullFeature = UCHAR_MAX; +/// The features that intersect to form the contact point +/// This must be 4 bytes or less. +struct b2ContactFeature +{ + enum Type + { + e_vertex = 0, + e_face = 1 + }; + + uint8 indexA; ///< Feature index on shapeA + uint8 indexB; ///< Feature index on shapeB + uint8 typeA; ///< The feature type on shapeA + uint8 typeB; ///< The feature type on shapeB +}; + /// Contact ids to facilitate warm starting. union b2ContactID { - /// The features that intersect to form the contact point - struct Features - { - uint8 referenceEdge; ///< The edge that defines the outward contact normal. - uint8 incidentEdge; ///< The edge most anti-parallel to the reference edge. - uint8 incidentVertex; ///< The vertex (0 or 1) on the incident edge that was clipped. - uint8 flip; ///< A value of 1 indicates that the reference edge is on shape2. - } features; + b2ContactFeature cf; uint32 key; ///< Used to quickly compare contact ids. }; @@ -107,7 +117,7 @@ struct b2WorldManifold const b2Transform& xfA, float32 radiusA, const b2Transform& xfB, float32 radiusB); - b2Vec2 normal; ///< world vector pointing from A to B + b2Vec2 normal; ///< world vector pointing from A to B b2Vec2 points[b2_maxManifoldPoints]; ///< world contact point (point of intersection) }; @@ -165,6 +175,21 @@ struct b2AABB return 0.5f * (upperBound - lowerBound); } + /// Get the perimeter length + float32 GetPerimeter() const + { + float32 wx = upperBound.x - lowerBound.x; + float32 wy = upperBound.y - lowerBound.y; + return 2.0f * (wx + wy); + } + + /// Combine an AABB into this one. + void Combine(const b2AABB& aabb) + { + lowerBound = b2Min(lowerBound, aabb.lowerBound); + upperBound = b2Max(upperBound, aabb.upperBound); + } + /// Combine two AABBs into this one. void Combine(const b2AABB& aabb1, const b2AABB& aabb2) { @@ -191,26 +216,37 @@ struct b2AABB /// Compute the collision manifold between two circles. void b2CollideCircles(b2Manifold* manifold, - const b2CircleShape* circle1, const b2Transform& xf1, - const b2CircleShape* circle2, const b2Transform& xf2); + const b2CircleShape* circleA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB); /// Compute the collision manifold between a polygon and a circle. void b2CollidePolygonAndCircle(b2Manifold* manifold, - const b2PolygonShape* polygon, const b2Transform& xf1, - const b2CircleShape* circle, const b2Transform& xf2); + const b2PolygonShape* polygonA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB); /// Compute the collision manifold between two polygons. void b2CollidePolygons(b2Manifold* manifold, - const b2PolygonShape* polygon1, const b2Transform& xf1, - const b2PolygonShape* polygon2, const b2Transform& xf2); + const b2PolygonShape* polygonA, const b2Transform& xfA, + const b2PolygonShape* polygonB, const b2Transform& xfB); + +/// Compute the collision manifold between an edge and a circle. +void b2CollideEdgeAndCircle(b2Manifold* manifold, + const b2EdgeShape* polygonA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB); + +/// Compute the collision manifold between an edge and a circle. +void b2CollideEdgeAndPolygon(b2Manifold* manifold, + const b2EdgeShape* edgeA, const b2Transform& xfA, + const b2PolygonShape* circleB, const b2Transform& xfB); /// Clipping for contact manifolds. int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], - const b2Vec2& normal, float32 offset); + const b2Vec2& normal, float32 offset, int32 vertexIndexA); /// Determine if two generic shapes overlap. -bool b2TestOverlap(const b2Shape* shapeA, const b2Shape* shapeB, - const b2Transform& xfA, const b2Transform& xfB); +bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, + const b2Shape* shapeB, int32 indexB, + const b2Transform& xfA, const b2Transform& xfB); // ---------------- Inline Functions ------------------------------------------ diff --git a/src/modules/physics/box2d/Box2D/Collision/b2Distance.cpp b/src/modules/physics/box2d/Box2D/Collision/b2Distance.cpp old mode 100644 new mode 100755 index f95c82f21..2bfb7598a --- a/src/modules/physics/box2d/Box2D/Collision/b2Distance.cpp +++ b/src/modules/physics/box2d/Box2D/Collision/b2Distance.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -18,12 +18,14 @@ #include #include +#include +#include #include // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters; -void b2DistanceProxy::Set(const b2Shape* shape) +void b2DistanceProxy::Set(const b2Shape* shape, int32 index) { switch (shape->GetType()) { @@ -45,6 +47,36 @@ void b2DistanceProxy::Set(const b2Shape* shape) } break; + case b2Shape::e_chain: + { + const b2ChainShape* chain = (b2ChainShape*)shape; + b2Assert(0 <= index && index < chain->GetVertexCount()); + + m_buffer[0] = chain->GetVertex(index); + if (index + 1 < chain->GetVertexCount()) + { + m_buffer[1] = chain->GetVertex(index + 1); + } + else + { + m_buffer[1] = chain->GetVertex(0); + } + + m_vertices = m_buffer; + m_count = 2; + m_radius = chain->m_radius; + } + break; + + case b2Shape::e_edge: + { + const b2EdgeShape* edge = (b2EdgeShape*)shape; + m_vertices = &edge->m_vertex1; + m_count = 2; + m_radius = edge->m_radius; + } + break; + default: b2Assert(false); } @@ -500,10 +532,10 @@ void b2Distance(b2DistanceOutput* output, // Compute a tentative new simplex vertex using support points. b2SimplexVertex* vertex = vertices + simplex.m_count; - vertex->indexA = proxyA->GetSupport(b2MulT(transformA.R, -d)); + vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d)); vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA)); b2Vec2 wBLocal; - vertex->indexB = proxyB->GetSupport(b2MulT(transformB.R, d)); + vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d)); vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB)); vertex->w = vertex->wB - vertex->wA; diff --git a/src/modules/physics/box2d/Box2D/Collision/b2Distance.h b/src/modules/physics/box2d/Box2D/Collision/b2Distance.h old mode 100644 new mode 100755 index e56ea0a7b..54ed1e136 --- a/src/modules/physics/box2d/Box2D/Collision/b2Distance.h +++ b/src/modules/physics/box2d/Box2D/Collision/b2Distance.h @@ -1,6 +1,6 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -21,7 +21,6 @@ #define B2_DISTANCE_H #include -#include class b2Shape; @@ -33,7 +32,7 @@ struct b2DistanceProxy /// Initialize the proxy using the given shape. The shape /// must remain in scope while the proxy is in use. - void Set(const b2Shape* shape); + void Set(const b2Shape* shape, int32 index); /// Get the supporting vertex index in the given direction. int32 GetSupport(const b2Vec2& d) const; @@ -47,6 +46,7 @@ struct b2DistanceProxy /// Get a vertex by index. Used by b2Distance. const b2Vec2& GetVertex(int32 index) const; + b2Vec2 m_buffer[2]; const b2Vec2* m_vertices; int32 m_count; float32 m_radius; diff --git a/src/modules/physics/box2d/Box2D/Collision/b2DynamicTree.cpp b/src/modules/physics/box2d/Box2D/Collision/b2DynamicTree.cpp old mode 100644 new mode 100755 index d8a05ebdf..1a42c3a5f --- a/src/modules/physics/box2d/Box2D/Collision/b2DynamicTree.cpp +++ b/src/modules/physics/box2d/Box2D/Collision/b2DynamicTree.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -19,6 +19,8 @@ #include #include #include +using namespace std; + b2DynamicTree::b2DynamicTree() { @@ -26,15 +28,17 @@ b2DynamicTree::b2DynamicTree() m_nodeCapacity = 16; m_nodeCount = 0; - m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode)); - memset(m_nodes, 0, m_nodeCapacity * sizeof(b2DynamicTreeNode)); + m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); + memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode)); // Build a linked list for the free list. for (int32 i = 0; i < m_nodeCapacity - 1; ++i) { m_nodes[i].next = i + 1; + m_nodes[i].height = -1; } m_nodes[m_nodeCapacity-1].next = b2_nullNode; + m_nodes[m_nodeCapacity-1].height = -1; m_freeList = 0; m_path = 0; @@ -57,10 +61,10 @@ int32 b2DynamicTree::AllocateNode() b2Assert(m_nodeCount == m_nodeCapacity); // The free list is empty. Rebuild a bigger pool. - b2DynamicTreeNode* oldNodes = m_nodes; + b2TreeNode* oldNodes = m_nodes; m_nodeCapacity *= 2; - m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode)); - memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2DynamicTreeNode)); + m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); + memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode)); b2Free(oldNodes); // Build a linked list for the free list. The parent @@ -68,8 +72,10 @@ int32 b2DynamicTree::AllocateNode() for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i) { m_nodes[i].next = i + 1; + m_nodes[i].height = -1; } m_nodes[m_nodeCapacity-1].next = b2_nullNode; + m_nodes[m_nodeCapacity-1].height = -1; m_freeList = m_nodeCount; } @@ -79,6 +85,8 @@ int32 b2DynamicTree::AllocateNode() m_nodes[nodeId].parent = b2_nullNode; m_nodes[nodeId].child1 = b2_nullNode; m_nodes[nodeId].child2 = b2_nullNode; + m_nodes[nodeId].height = 0; + m_nodes[nodeId].userData = NULL; ++m_nodeCount; return nodeId; } @@ -89,6 +97,7 @@ void b2DynamicTree::FreeNode(int32 nodeId) b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); b2Assert(0 < m_nodeCount); m_nodes[nodeId].next = m_freeList; + m_nodes[nodeId].height = -1; m_freeList = nodeId; --m_nodeCount; } @@ -105,20 +114,10 @@ int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData) m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r; m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r; m_nodes[proxyId].userData = userData; + m_nodes[proxyId].height = 0; InsertLeaf(proxyId); - // Rebalance if necessary. - int32 iterationCount = m_nodeCount >> 4; - int32 tryCount = 0; - int32 height = ComputeHeight(); - while (height > 64 && tryCount < 10) - { - Rebalance(iterationCount); - height = ComputeHeight(); - ++tryCount; - } - return proxyId; } @@ -188,79 +187,133 @@ void b2DynamicTree::InsertLeaf(int32 leaf) return; } - // Find the best sibling for this node. - b2Vec2 center = m_nodes[leaf].aabb.GetCenter(); - int32 sibling = m_root; - if (m_nodes[sibling].IsLeaf() == false) + // Find the best sibling for this node + b2AABB leafAABB = m_nodes[leaf].aabb; + int32 index = m_root; + while (m_nodes[index].IsLeaf() == false) { - do + int32 child1 = m_nodes[index].child1; + int32 child2 = m_nodes[index].child2; + + float32 area = m_nodes[index].aabb.GetPerimeter(); + + b2AABB combinedAABB; + combinedAABB.Combine(m_nodes[index].aabb, leafAABB); + float32 combinedArea = combinedAABB.GetPerimeter(); + + // Cost of creating a new parent for this node and the new leaf + float32 cost = 2.0f * combinedArea; + + // Minimum cost of pushing the leaf further down the tree + float32 inheritanceCost = 2.0f * (combinedArea - area); + + // Cost of descending into child1 + float32 cost1; + if (m_nodes[child1].IsLeaf()) { - int32 child1 = m_nodes[sibling].child1; - int32 child2 = m_nodes[sibling].child2; - - b2Vec2 delta1 = b2Abs(m_nodes[child1].aabb.GetCenter() - center); - b2Vec2 delta2 = b2Abs(m_nodes[child2].aabb.GetCenter() - center); - - float32 norm1 = delta1.x + delta1.y; - float32 norm2 = delta2.x + delta2.y; - - if (norm1 < norm2) - { - sibling = child1; - } - else - { - sibling = child2; - } - - } - while(m_nodes[sibling].IsLeaf() == false); - } - - // Create a parent for the siblings. - int32 node1 = m_nodes[sibling].parent; - int32 node2 = AllocateNode(); - m_nodes[node2].parent = node1; - m_nodes[node2].userData = NULL; - m_nodes[node2].aabb.Combine(m_nodes[leaf].aabb, m_nodes[sibling].aabb); - - if (node1 != b2_nullNode) - { - if (m_nodes[m_nodes[sibling].parent].child1 == sibling) - { - m_nodes[node1].child1 = node2; + b2AABB aabb; + aabb.Combine(leafAABB, m_nodes[child1].aabb); + cost1 = aabb.GetPerimeter() + inheritanceCost; } else { - m_nodes[node1].child2 = node2; + b2AABB aabb; + aabb.Combine(leafAABB, m_nodes[child1].aabb); + float32 oldArea = m_nodes[child1].aabb.GetPerimeter(); + float32 newArea = aabb.GetPerimeter(); + cost1 = (newArea - oldArea) + inheritanceCost; } - m_nodes[node2].child1 = sibling; - m_nodes[node2].child2 = leaf; - m_nodes[sibling].parent = node2; - m_nodes[leaf].parent = node2; - - do + // Cost of descending into child2 + float32 cost2; + if (m_nodes[child2].IsLeaf()) { - if (m_nodes[node1].aabb.Contains(m_nodes[node2].aabb)) - { - break; - } - - m_nodes[node1].aabb.Combine(m_nodes[m_nodes[node1].child1].aabb, m_nodes[m_nodes[node1].child2].aabb); - node2 = node1; - node1 = m_nodes[node1].parent; + b2AABB aabb; + aabb.Combine(leafAABB, m_nodes[child2].aabb); + cost2 = aabb.GetPerimeter() + inheritanceCost; } - while(node1 != b2_nullNode); + else + { + b2AABB aabb; + aabb.Combine(leafAABB, m_nodes[child2].aabb); + float32 oldArea = m_nodes[child2].aabb.GetPerimeter(); + float32 newArea = aabb.GetPerimeter(); + cost2 = newArea - oldArea + inheritanceCost; + } + + // Descend according to the minimum cost. + if (cost < cost1 && cost < cost2) + { + break; + } + + // Descend + if (cost1 < cost2) + { + index = child1; + } + else + { + index = child2; + } + } + + int32 sibling = index; + + // Create a new parent. + int32 oldParent = m_nodes[sibling].parent; + int32 newParent = AllocateNode(); + m_nodes[newParent].parent = oldParent; + m_nodes[newParent].userData = NULL; + m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb); + m_nodes[newParent].height = m_nodes[sibling].height + 1; + + if (oldParent != b2_nullNode) + { + // The sibling was not the root. + if (m_nodes[oldParent].child1 == sibling) + { + m_nodes[oldParent].child1 = newParent; + } + else + { + m_nodes[oldParent].child2 = newParent; + } + + m_nodes[newParent].child1 = sibling; + m_nodes[newParent].child2 = leaf; + m_nodes[sibling].parent = newParent; + m_nodes[leaf].parent = newParent; } else { - m_nodes[node2].child1 = sibling; - m_nodes[node2].child2 = leaf; - m_nodes[sibling].parent = node2; - m_nodes[leaf].parent = node2; - m_root = node2; + // The sibling was the root. + m_nodes[newParent].child1 = sibling; + m_nodes[newParent].child2 = leaf; + m_nodes[sibling].parent = newParent; + m_nodes[leaf].parent = newParent; + m_root = newParent; } + + // Walk back up the tree fixing heights and AABBs + index = m_nodes[leaf].parent; + while (index != b2_nullNode) + { + index = Balance(index); + + int32 child1 = m_nodes[index].child1; + int32 child2 = m_nodes[index].child2; + + b2Assert(child1 != b2_nullNode); + b2Assert(child2 != b2_nullNode); + + m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); + m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); + + index = m_nodes[index].parent; + } + + //Validate(); } void b2DynamicTree::RemoveLeaf(int32 leaf) @@ -271,89 +324,250 @@ void b2DynamicTree::RemoveLeaf(int32 leaf) return; } - int32 node2 = m_nodes[leaf].parent; - int32 node1 = m_nodes[node2].parent; + int32 parent = m_nodes[leaf].parent; + int32 grandParent = m_nodes[parent].parent; int32 sibling; - if (m_nodes[node2].child1 == leaf) + if (m_nodes[parent].child1 == leaf) { - sibling = m_nodes[node2].child2; + sibling = m_nodes[parent].child2; } else { - sibling = m_nodes[node2].child1; + sibling = m_nodes[parent].child1; } - if (node1 != b2_nullNode) + if (grandParent != b2_nullNode) { - // Destroy node2 and connect node1 to sibling. - if (m_nodes[node1].child1 == node2) + // Destroy parent and connect sibling to grandParent. + if (m_nodes[grandParent].child1 == parent) { - m_nodes[node1].child1 = sibling; + m_nodes[grandParent].child1 = sibling; } else { - m_nodes[node1].child2 = sibling; + m_nodes[grandParent].child2 = sibling; } - m_nodes[sibling].parent = node1; - FreeNode(node2); + m_nodes[sibling].parent = grandParent; + FreeNode(parent); // Adjust ancestor bounds. - while (node1 != b2_nullNode) + int32 index = grandParent; + while (index != b2_nullNode) { - b2AABB oldAABB = m_nodes[node1].aabb; - m_nodes[node1].aabb.Combine(m_nodes[m_nodes[node1].child1].aabb, m_nodes[m_nodes[node1].child2].aabb); + index = Balance(index); - if (oldAABB.Contains(m_nodes[node1].aabb)) - { - break; - } + int32 child1 = m_nodes[index].child1; + int32 child2 = m_nodes[index].child2; - node1 = m_nodes[node1].parent; + m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); + m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); + + index = m_nodes[index].parent; } } else { m_root = sibling; m_nodes[sibling].parent = b2_nullNode; - FreeNode(node2); + FreeNode(parent); } + + //Validate(); } -void b2DynamicTree::Rebalance(int32 iterations) +// Perform a left or right rotation if node A is imbalanced. +// Returns the new root index. +int32 b2DynamicTree::Balance(int32 iA) +{ + b2Assert(iA != b2_nullNode); + + b2TreeNode* A = m_nodes + iA; + if (A->IsLeaf() || A->height < 2) + { + return iA; + } + + int32 iB = A->child1; + int32 iC = A->child2; + b2Assert(0 <= iB && iB < m_nodeCapacity); + b2Assert(0 <= iC && iC < m_nodeCapacity); + + b2TreeNode* B = m_nodes + iB; + b2TreeNode* C = m_nodes + iC; + + int32 balance = C->height - B->height; + + // Rotate C up + if (balance > 1) + { + int32 iF = C->child1; + int32 iG = C->child2; + b2TreeNode* F = m_nodes + iF; + b2TreeNode* G = m_nodes + iG; + b2Assert(0 <= iF && iF < m_nodeCapacity); + b2Assert(0 <= iG && iG < m_nodeCapacity); + + // Swap A and C + C->child1 = iA; + C->parent = A->parent; + A->parent = iC; + + // A's old parent should point to C + if (C->parent != b2_nullNode) + { + if (m_nodes[C->parent].child1 == iA) + { + m_nodes[C->parent].child1 = iC; + } + else + { + b2Assert(m_nodes[C->parent].child2 == iA); + m_nodes[C->parent].child2 = iC; + } + } + else + { + m_root = iC; + } + + // Rotate + if (F->height > G->height) + { + C->child2 = iF; + A->child2 = iG; + G->parent = iA; + A->aabb.Combine(B->aabb, G->aabb); + C->aabb.Combine(A->aabb, F->aabb); + + A->height = 1 + b2Max(B->height, G->height); + C->height = 1 + b2Max(A->height, F->height); + } + else + { + C->child2 = iG; + A->child2 = iF; + F->parent = iA; + A->aabb.Combine(B->aabb, F->aabb); + C->aabb.Combine(A->aabb, G->aabb); + + A->height = 1 + b2Max(B->height, F->height); + C->height = 1 + b2Max(A->height, G->height); + } + + return iC; + } + + // Rotate B up + if (balance < -1) + { + int32 iD = B->child1; + int32 iE = B->child2; + b2TreeNode* D = m_nodes + iD; + b2TreeNode* E = m_nodes + iE; + b2Assert(0 <= iD && iD < m_nodeCapacity); + b2Assert(0 <= iE && iE < m_nodeCapacity); + + // Swap A and B + B->child1 = iA; + B->parent = A->parent; + A->parent = iB; + + // A's old parent should point to B + if (B->parent != b2_nullNode) + { + if (m_nodes[B->parent].child1 == iA) + { + m_nodes[B->parent].child1 = iB; + } + else + { + b2Assert(m_nodes[B->parent].child2 == iA); + m_nodes[B->parent].child2 = iB; + } + } + else + { + m_root = iB; + } + + // Rotate + if (D->height > E->height) + { + B->child2 = iD; + A->child1 = iE; + E->parent = iA; + A->aabb.Combine(C->aabb, E->aabb); + B->aabb.Combine(A->aabb, D->aabb); + + A->height = 1 + b2Max(C->height, E->height); + B->height = 1 + b2Max(A->height, D->height); + } + else + { + B->child2 = iE; + A->child1 = iD; + D->parent = iA; + A->aabb.Combine(C->aabb, D->aabb); + B->aabb.Combine(A->aabb, E->aabb); + + A->height = 1 + b2Max(C->height, D->height); + B->height = 1 + b2Max(A->height, E->height); + } + + return iB; + } + + return iA; +} + +int32 b2DynamicTree::GetHeight() const { if (m_root == b2_nullNode) { - return; + return 0; } - for (int32 i = 0; i < iterations; ++i) + return m_nodes[m_root].height; +} + +// +float32 b2DynamicTree::GetAreaRatio() const +{ + if (m_root == b2_nullNode) { - int32 node = m_root; - - uint32 bit = 0; - while (m_nodes[node].IsLeaf() == false) - { - int32* children = &m_nodes[node].child1; - node = children[(m_path >> bit) & 1]; - bit = (bit + 1) & (8* sizeof(uint32) - 1); - } - ++m_path; - - RemoveLeaf(node); - InsertLeaf(node); + return 0.0f; } + + const b2TreeNode* root = m_nodes + m_root; + float32 rootArea = root->aabb.GetPerimeter(); + + float32 totalArea = 0.0f; + for (int32 i = 0; i < m_nodeCapacity; ++i) + { + const b2TreeNode* node = m_nodes + i; + if (node->height < 0) + { + // Free node in pool + continue; + } + + totalArea += node->aabb.GetPerimeter(); + } + + return totalArea / rootArea; } // Compute the height of a sub-tree. int32 b2DynamicTree::ComputeHeight(int32 nodeId) const { - if (nodeId == b2_nullNode) + b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); + b2TreeNode* node = m_nodes + nodeId; + + if (node->IsLeaf()) { return 0; } - b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); - b2DynamicTreeNode* node = m_nodes + nodeId; int32 height1 = ComputeHeight(node->child1); int32 height2 = ComputeHeight(node->child2); return 1 + b2Max(height1, height2); @@ -361,5 +575,197 @@ int32 b2DynamicTree::ComputeHeight(int32 nodeId) const int32 b2DynamicTree::ComputeHeight() const { - return ComputeHeight(m_root); + int32 height = ComputeHeight(m_root); + return height; +} + +void b2DynamicTree::ValidateStructure(int32 index) const +{ + if (index == b2_nullNode) + { + return; + } + + if (index == m_root) + { + b2Assert(m_nodes[index].parent == b2_nullNode); + } + + const b2TreeNode* node = m_nodes + index; + + int32 child1 = node->child1; + int32 child2 = node->child2; + + if (node->IsLeaf()) + { + b2Assert(child1 == b2_nullNode); + b2Assert(child2 == b2_nullNode); + b2Assert(node->height == 0); + return; + } + + b2Assert(0 <= child1 && child1 < m_nodeCapacity); + b2Assert(0 <= child2 && child2 < m_nodeCapacity); + + b2Assert(m_nodes[child1].parent == index); + b2Assert(m_nodes[child2].parent == index); + + ValidateStructure(child1); + ValidateStructure(child2); +} + +void b2DynamicTree::ValidateMetrics(int32 index) const +{ + if (index == b2_nullNode) + { + return; + } + + const b2TreeNode* node = m_nodes + index; + + int32 child1 = node->child1; + int32 child2 = node->child2; + + if (node->IsLeaf()) + { + b2Assert(child1 == b2_nullNode); + b2Assert(child2 == b2_nullNode); + b2Assert(node->height == 0); + return; + } + + b2Assert(0 <= child1 && child1 < m_nodeCapacity); + b2Assert(0 <= child2 && child2 < m_nodeCapacity); + + int32 height1 = m_nodes[child1].height; + int32 height2 = m_nodes[child2].height; + int32 height; + height = 1 + b2Max(height1, height2); + b2Assert(node->height == height); + + b2AABB aabb; + aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); + + b2Assert(aabb.lowerBound == node->aabb.lowerBound); + b2Assert(aabb.upperBound == node->aabb.upperBound); + + ValidateMetrics(child1); + ValidateMetrics(child2); +} + +void b2DynamicTree::Validate() const +{ + ValidateStructure(m_root); + ValidateMetrics(m_root); + + int32 freeCount = 0; + int32 freeIndex = m_freeList; + while (freeIndex != b2_nullNode) + { + b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity); + freeIndex = m_nodes[freeIndex].next; + ++freeCount; + } + + b2Assert(GetHeight() == ComputeHeight()); + + b2Assert(m_nodeCount + freeCount == m_nodeCapacity); +} + +int32 b2DynamicTree::GetMaxBalance() const +{ + int32 maxBalance = 0; + for (int32 i = 0; i < m_nodeCapacity; ++i) + { + const b2TreeNode* node = m_nodes + i; + if (node->height <= 1) + { + continue; + } + + b2Assert(node->IsLeaf() == false); + + int32 child1 = node->child1; + int32 child2 = node->child2; + int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height); + maxBalance = b2Max(maxBalance, balance); + } + + return maxBalance; +} + +void b2DynamicTree::RebuildBottomUp() +{ + int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32)); + int32 count = 0; + + // Build array of leaves. Free the rest. + for (int32 i = 0; i < m_nodeCapacity; ++i) + { + if (m_nodes[i].height < 0) + { + // free node in pool + continue; + } + + if (m_nodes[i].IsLeaf()) + { + m_nodes[i].parent = b2_nullNode; + nodes[count] = i; + ++count; + } + else + { + FreeNode(i); + } + } + + while (count > 1) + { + float32 minCost = b2_maxFloat; + int32 iMin = -1, jMin = -1; + for (int32 i = 0; i < count; ++i) + { + b2AABB aabbi = m_nodes[nodes[i]].aabb; + + for (int32 j = i + 1; j < count; ++j) + { + b2AABB aabbj = m_nodes[nodes[j]].aabb; + b2AABB b; + b.Combine(aabbi, aabbj); + float32 cost = b.GetPerimeter(); + if (cost < minCost) + { + iMin = i; + jMin = j; + minCost = cost; + } + } + } + + int32 index1 = nodes[iMin]; + int32 index2 = nodes[jMin]; + b2TreeNode* child1 = m_nodes + index1; + b2TreeNode* child2 = m_nodes + index2; + + int32 parentIndex = AllocateNode(); + b2TreeNode* parent = m_nodes + parentIndex; + parent->child1 = index1; + parent->child2 = index2; + parent->height = 1 + b2Max(child1->height, child2->height); + parent->aabb.Combine(child1->aabb, child2->aabb); + parent->parent = b2_nullNode; + + child1->parent = parentIndex; + child2->parent = parentIndex; + + nodes[jMin] = nodes[count-1]; + nodes[iMin] = parentIndex; + --count; + } + + m_root = nodes[0]; + b2Free(nodes); + + Validate(); } diff --git a/src/modules/physics/box2d/Box2D/Collision/b2DynamicTree.h b/src/modules/physics/box2d/Box2D/Collision/b2DynamicTree.h old mode 100644 new mode 100755 index b67686b5c..a9bfbf379 --- a/src/modules/physics/box2d/Box2D/Collision/b2DynamicTree.h +++ b/src/modules/physics/box2d/Box2D/Collision/b2DynamicTree.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -20,23 +20,21 @@ #define B2_DYNAMIC_TREE_H #include - -/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. +#include #define b2_nullNode (-1) /// A node in the dynamic tree. The client does not interact with this directly. -struct b2DynamicTreeNode +struct b2TreeNode { bool IsLeaf() const { return child1 == b2_nullNode; } - /// This is the fattened AABB. + /// Enlarged AABB b2AABB aabb; - //int32 userData; void* userData; union @@ -47,8 +45,12 @@ struct b2DynamicTreeNode int32 child1; int32 child2; + + // leaf = 0, free node = -1 + int32 height; }; +/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. /// A dynamic tree arranges data in a binary tree to accelerate /// queries such as volume queries and ray casts. Leafs are proxies /// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor @@ -59,7 +61,6 @@ struct b2DynamicTreeNode class b2DynamicTree { public: - /// Constructing the tree initializes the node pool. b2DynamicTree(); @@ -78,9 +79,6 @@ public: /// @return true if the proxy was re-inserted. bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement); - /// Perform some iterations to re-balance the tree. - void Rebalance(int32 iterations); - /// Get proxy user data. /// @return the proxy user data or 0 if the id is invalid. void* GetUserData(int32 proxyId) const; @@ -88,9 +86,6 @@ public: /// Get the fat AABB for a proxy. const b2AABB& GetFatAABB(int32 proxyId) const; - /// Compute the height of the tree. - int32 ComputeHeight() const; - /// Query an AABB for overlapping proxies. The callback class /// is called for each proxy that overlaps the supplied AABB. template @@ -106,6 +101,23 @@ public: template void RayCast(T* callback, const b2RayCastInput& input) const; + /// Validate this tree. For testing. + void Validate() const; + + /// Compute the height of the binary tree in O(N) time. Should not be + /// called often. + int32 GetHeight() const; + + /// Get the maximum balance of an node in the tree. The balance is the difference + /// in height of the two children of a node. + int32 GetMaxBalance() const; + + /// Get the ratio of the sum of the node areas to the root area. + float32 GetAreaRatio() const; + + /// Build an optimal tree. Very expensive. For testing. + void RebuildBottomUp(); + private: int32 AllocateNode(); @@ -114,17 +126,23 @@ private: void InsertLeaf(int32 node); void RemoveLeaf(int32 node); + int32 Balance(int32 index); + + int32 ComputeHeight() const; int32 ComputeHeight(int32 nodeId) const; + void ValidateStructure(int32 index) const; + void ValidateMetrics(int32 index) const; + int32 m_root; - b2DynamicTreeNode* m_nodes; + b2TreeNode* m_nodes; int32 m_nodeCount; int32 m_nodeCapacity; int32 m_freeList; - /// This is used incrementally traverse the tree for re-balancing. + /// This is used to incrementally traverse the tree for re-balancing. uint32 m_path; int32 m_insertionCount; @@ -145,21 +163,18 @@ inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const template inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const { - const int32 k_stackSize = 128; - int32 stack[k_stackSize]; + b2GrowableStack stack; + stack.Push(m_root); - int32 count = 0; - stack[count++] = m_root; - - while (count > 0) + while (stack.GetCount() > 0) { - int32 nodeId = stack[--count]; + int32 nodeId = stack.Pop(); if (nodeId == b2_nullNode) { continue; } - const b2DynamicTreeNode* node = m_nodes + nodeId; + const b2TreeNode* node = m_nodes + nodeId; if (b2TestOverlap(node->aabb, aabb)) { @@ -173,15 +188,8 @@ inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const } else { - if (count < k_stackSize) - { - stack[count++] = node->child1; - } - - if (count < k_stackSize) - { - stack[count++] = node->child2; - } + stack.Push(node->child1); + stack.Push(node->child2); } } } @@ -213,21 +221,18 @@ inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) con segmentAABB.upperBound = b2Max(p1, t); } - const int32 k_stackSize = 128; - int32 stack[k_stackSize]; + b2GrowableStack stack; + stack.Push(m_root); - int32 count = 0; - stack[count++] = m_root; - - while (count > 0) + while (stack.GetCount() > 0) { - int32 nodeId = stack[--count]; + int32 nodeId = stack.Pop(); if (nodeId == b2_nullNode) { continue; } - const b2DynamicTreeNode* node = m_nodes + nodeId; + const b2TreeNode* node = m_nodes + nodeId; if (b2TestOverlap(node->aabb, segmentAABB) == false) { @@ -270,15 +275,8 @@ inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) con } else { - if (count < k_stackSize) - { - stack[count++] = node->child1; - } - - if (count < k_stackSize) - { - stack[count++] = node->child2; - } + stack.Push(node->child1); + stack.Push(node->child2); } } } diff --git a/src/modules/physics/box2d/Box2D/Collision/b2TimeOfImpact.cpp b/src/modules/physics/box2d/Box2D/Collision/b2TimeOfImpact.cpp old mode 100644 new mode 100755 index b1f2f4e5a..5c33e828c --- a/src/modules/physics/box2d/Box2D/Collision/b2TimeOfImpact.cpp +++ b/src/modules/physics/box2d/Box2D/Collision/b2TimeOfImpact.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -23,12 +23,11 @@ #include #include +using namespace std; int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters; int32 b2_toiRootIters, b2_toiMaxRootIters; -int32 b2_toiMaxOptIters; - struct b2SeparationFunction { enum Type @@ -42,7 +41,8 @@ struct b2SeparationFunction float32 Initialize(const b2SimplexCache* cache, const b2DistanceProxy* proxyA, const b2Sweep& sweepA, - const b2DistanceProxy* proxyB, const b2Sweep& sweepB) + const b2DistanceProxy* proxyB, const b2Sweep& sweepB, + float32 t1) { m_proxyA = proxyA; m_proxyB = proxyB; @@ -53,8 +53,8 @@ struct b2SeparationFunction m_sweepB = sweepB; b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, 0.0f); - m_sweepB.GetTransform(&xfB, 0.0f); + m_sweepA.GetTransform(&xfA, t1); + m_sweepB.GetTransform(&xfB, t1); if (count == 1) { @@ -76,7 +76,7 @@ struct b2SeparationFunction m_axis = b2Cross(localPointB2 - localPointB1, 1.0f); m_axis.Normalize(); - b2Vec2 normal = b2Mul(xfB.R, m_axis); + b2Vec2 normal = b2Mul(xfB.q, m_axis); m_localPoint = 0.5f * (localPointB1 + localPointB2); b2Vec2 pointB = b2Mul(xfB, m_localPoint); @@ -101,7 +101,7 @@ struct b2SeparationFunction m_axis = b2Cross(localPointA2 - localPointA1, 1.0f); m_axis.Normalize(); - b2Vec2 normal = b2Mul(xfA.R, m_axis); + b2Vec2 normal = b2Mul(xfA.q, m_axis); m_localPoint = 0.5f * (localPointA1 + localPointA2); b2Vec2 pointA = b2Mul(xfA, m_localPoint); @@ -129,8 +129,8 @@ struct b2SeparationFunction { case e_points: { - b2Vec2 axisA = b2MulT(xfA.R, m_axis); - b2Vec2 axisB = b2MulT(xfB.R, -m_axis); + b2Vec2 axisA = b2MulT(xfA.q, m_axis); + b2Vec2 axisB = b2MulT(xfB.q, -m_axis); *indexA = m_proxyA->GetSupport(axisA); *indexB = m_proxyB->GetSupport(axisB); @@ -147,10 +147,10 @@ struct b2SeparationFunction case e_faceA: { - b2Vec2 normal = b2Mul(xfA.R, m_axis); + b2Vec2 normal = b2Mul(xfA.q, m_axis); b2Vec2 pointA = b2Mul(xfA, m_localPoint); - b2Vec2 axisB = b2MulT(xfB.R, -normal); + b2Vec2 axisB = b2MulT(xfB.q, -normal); *indexA = -1; *indexB = m_proxyB->GetSupport(axisB); @@ -164,10 +164,10 @@ struct b2SeparationFunction case e_faceB: { - b2Vec2 normal = b2Mul(xfB.R, m_axis); + b2Vec2 normal = b2Mul(xfB.q, m_axis); b2Vec2 pointB = b2Mul(xfB, m_localPoint); - b2Vec2 axisA = b2MulT(xfA.R, -normal); + b2Vec2 axisA = b2MulT(xfA.q, -normal); *indexB = -1; *indexA = m_proxyA->GetSupport(axisA); @@ -197,8 +197,8 @@ struct b2SeparationFunction { case e_points: { - b2Vec2 axisA = b2MulT(xfA.R, m_axis); - b2Vec2 axisB = b2MulT(xfB.R, -m_axis); + b2Vec2 axisA = b2MulT(xfA.q, m_axis); + b2Vec2 axisB = b2MulT(xfB.q, -m_axis); b2Vec2 localPointA = m_proxyA->GetVertex(indexA); b2Vec2 localPointB = m_proxyB->GetVertex(indexB); @@ -212,10 +212,10 @@ struct b2SeparationFunction case e_faceA: { - b2Vec2 normal = b2Mul(xfA.R, m_axis); + b2Vec2 normal = b2Mul(xfA.q, m_axis); b2Vec2 pointA = b2Mul(xfA, m_localPoint); - b2Vec2 axisB = b2MulT(xfB.R, -normal); + b2Vec2 axisB = b2MulT(xfB.q, -normal); b2Vec2 localPointB = m_proxyB->GetVertex(indexB); b2Vec2 pointB = b2Mul(xfB, localPointB); @@ -226,10 +226,10 @@ struct b2SeparationFunction case e_faceB: { - b2Vec2 normal = b2Mul(xfB.R, m_axis); + b2Vec2 normal = b2Mul(xfB.q, m_axis); b2Vec2 pointB = b2Mul(xfB, m_localPoint); - b2Vec2 axisA = b2MulT(xfA.R, -normal); + b2Vec2 axisA = b2MulT(xfA.q, -normal); b2Vec2 localPointA = m_proxyA->GetVertex(indexA); b2Vec2 pointA = b2Mul(xfA, localPointA); @@ -325,7 +325,7 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) // Initialize the separating axis. b2SeparationFunction fcn; - fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB); + fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1); #if 0 // Dump the curve seen by the root finder { diff --git a/src/modules/physics/box2d/Box2D/Collision/b2TimeOfImpact.h b/src/modules/physics/box2d/Box2D/Collision/b2TimeOfImpact.h old mode 100644 new mode 100755 index b59fb831c..179a1700e --- a/src/modules/physics/box2d/Box2D/Collision/b2TimeOfImpact.h +++ b/src/modules/physics/box2d/Box2D/Collision/b2TimeOfImpact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -21,7 +21,6 @@ #include #include -#include /// Input parameters for b2TimeOfImpact struct b2TOIInput diff --git a/src/modules/physics/box2d/Box2D/Common/b2BlockAllocator.cpp b/src/modules/physics/box2d/Box2D/Common/b2BlockAllocator.cpp old mode 100644 new mode 100755 index ba06f0494..f5060daa2 --- a/src/modules/physics/box2d/Box2D/Common/b2BlockAllocator.cpp +++ b/src/modules/physics/box2d/Box2D/Common/b2BlockAllocator.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -21,6 +21,7 @@ #include #include #include +using namespace std; int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = { @@ -100,7 +101,12 @@ void* b2BlockAllocator::Allocate(int32 size) if (size == 0) return NULL; - b2Assert(0 < size && size <= b2_maxBlockSize); + b2Assert(0 < size); + + if (size > b2_maxBlockSize) + { + return b2Alloc(size); + } int32 index = s_blockSizeLookup[size]; b2Assert(0 <= index && index < b2_blockSizes); @@ -155,7 +161,13 @@ void b2BlockAllocator::Free(void* p, int32 size) return; } - b2Assert(0 < size && size <= b2_maxBlockSize); + b2Assert(0 < size); + + if (size > b2_maxBlockSize) + { + b2Free(p); + return; + } int32 index = s_blockSizeLookup[size]; b2Assert(0 <= index && index < b2_blockSizes); diff --git a/src/modules/physics/box2d/Box2D/Common/b2BlockAllocator.h b/src/modules/physics/box2d/Box2D/Common/b2BlockAllocator.h old mode 100644 new mode 100755 index 93eb2e311..8ba29a5e9 --- a/src/modules/physics/box2d/Box2D/Common/b2BlockAllocator.h +++ b/src/modules/physics/box2d/Box2D/Common/b2BlockAllocator.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -21,7 +21,7 @@ #include -const int32 b2_chunkSize = 4096; +const int32 b2_chunkSize = 16 * 1024; const int32 b2_maxBlockSize = 640; const int32 b2_blockSizes = 14; const int32 b2_chunkArrayIncrement = 128; @@ -29,16 +29,19 @@ const int32 b2_chunkArrayIncrement = 128; struct b2Block; struct b2Chunk; -// This is a small object allocator used for allocating small -// objects that persist for more than one time step. -// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp +/// This is a small object allocator used for allocating small +/// objects that persist for more than one time step. +/// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp class b2BlockAllocator { public: b2BlockAllocator(); ~b2BlockAllocator(); + /// Allocate memory. This will use b2Alloc if the size is larger than b2_maxBlockSize. void* Allocate(int32 size); + + /// Free memory. This will use b2Free if the size is larger than b2_maxBlockSize. void Free(void* p, int32 size); void Clear(); diff --git a/src/modules/physics/box2d/Box2D/Common/b2Draw.cpp b/src/modules/physics/box2d/Box2D/Common/b2Draw.cpp new file mode 100755 index 000000000..327b58079 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Common/b2Draw.cpp @@ -0,0 +1,44 @@ +/* +* Copyright (c) 2011 Erin Catto http://box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include + +b2Draw::b2Draw() +{ + m_drawFlags = 0; +} + +void b2Draw::SetFlags(uint32 flags) +{ + m_drawFlags = flags; +} + +uint32 b2Draw::GetFlags() const +{ + return m_drawFlags; +} + +void b2Draw::AppendFlags(uint32 flags) +{ + m_drawFlags |= flags; +} + +void b2Draw::ClearFlags(uint32 flags) +{ + m_drawFlags &= ~flags; +} diff --git a/src/modules/physics/box2d/Box2D/Common/b2Draw.h b/src/modules/physics/box2d/Box2D/Common/b2Draw.h new file mode 100755 index 000000000..de62dd611 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Common/b2Draw.h @@ -0,0 +1,81 @@ +/* +* Copyright (c) 2011 Erin Catto http://box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include + +/// Color for debug drawing. Each value has the range [0,1]. +struct b2Color +{ + b2Color() {} + b2Color(float32 r, float32 g, float32 b) : r(r), g(g), b(b) {} + void Set(float32 ri, float32 gi, float32 bi) { r = ri; g = gi; b = bi; } + float32 r, g, b; +}; + +/// Implement and register this class with a b2World to provide debug drawing of physics +/// entities in your game. +class b2Draw +{ +public: + b2Draw(); + + virtual ~b2Draw() {} + + enum + { + e_shapeBit = 0x0001, ///< draw shapes + e_jointBit = 0x0002, ///< draw joint connections + e_aabbBit = 0x0004, ///< draw axis aligned bounding boxes + e_pairBit = 0x0008, ///< draw broad-phase pairs + e_centerOfMassBit = 0x0010 ///< draw center of mass frame + }; + + /// Set the drawing flags. + void SetFlags(uint32 flags); + + /// Get the drawing flags. + uint32 GetFlags() const; + + /// Append flags to the current flags. + void AppendFlags(uint32 flags); + + /// Clear flags from the current flags. + void ClearFlags(uint32 flags); + + /// Draw a closed polygon provided in CCW order. + virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; + + /// Draw a solid closed polygon provided in CCW order. + virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; + + /// Draw a circle. + virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0; + + /// Draw a solid circle. + virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0; + + /// Draw a line segment. + virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0; + + /// Draw a transform. Choose your own length scale. + /// @param xf a transform. + virtual void DrawTransform(const b2Transform& xf) = 0; + +protected: + uint32 m_drawFlags; +}; diff --git a/src/modules/physics/box2d/Box2D/Common/b2GrowableStack.h b/src/modules/physics/box2d/Box2D/Common/b2GrowableStack.h new file mode 100755 index 000000000..d2e07120f --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Common/b2GrowableStack.h @@ -0,0 +1,85 @@ +/* +* Copyright (c) 2010 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B2_GROWABLE_STACK_H +#define B2_GROWABLE_STACK_H +#include +#include + +/// This is a growable LIFO stack with an initial capacity of N. +/// If the stack size exceeds the initial capacity, the heap is used +/// to increase the size of the stack. +template +class b2GrowableStack +{ +public: + b2GrowableStack() + { + m_stack = m_array; + m_count = 0; + m_capacity = N; + } + + ~b2GrowableStack() + { + if (m_stack != m_array) + { + b2Free(m_stack); + m_stack = NULL; + } + } + + void Push(const T& element) + { + if (m_count == m_capacity) + { + T* old = m_stack; + m_capacity *= 2; + m_stack = (T*)b2Alloc(m_capacity * sizeof(T)); + std::memcpy(m_stack, old, m_count * sizeof(T)); + if (old != m_array) + { + b2Free(old); + } + } + + m_stack[m_count] = element; + ++m_count; + } + + T Pop() + { + b2Assert(m_count > 0); + --m_count; + return m_stack[m_count]; + } + + int32 GetCount() + { + return m_count; + } + +private: + T* m_stack; + T m_array[N]; + int32 m_count; + int32 m_capacity; +}; + + +#endif diff --git a/src/modules/physics/box2d/Box2D/Common/b2Math.cpp b/src/modules/physics/box2d/Box2D/Common/b2Math.cpp old mode 100644 new mode 100755 index f15a43bd5..b06111dc4 --- a/src/modules/physics/box2d/Box2D/Common/b2Math.cpp +++ b/src/modules/physics/box2d/Box2D/Common/b2Math.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -19,22 +19,20 @@ #include const b2Vec2 b2Vec2_zero(0.0f, 0.0f); -const b2Mat22 b2Mat22_identity(1.0f, 0.0f, 0.0f, 1.0f); -const b2Transform b2Transform_identity(b2Vec2_zero, b2Mat22_identity); /// Solve A * x = b, where b is a column vector. This is more efficient /// than computing the inverse in one-shot cases. b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const { - float32 det = b2Dot(col1, b2Cross(col2, col3)); + float32 det = b2Dot(ex, b2Cross(ey, ez)); if (det != 0.0f) { det = 1.0f / det; } b2Vec3 x; - x.x = det * b2Dot(b, b2Cross(col2, col3)); - x.y = det * b2Dot(col1, b2Cross(b, col3)); - x.z = det * b2Dot(col1, b2Cross(col2, b)); + x.x = det * b2Dot(b, b2Cross(ey, ez)); + x.y = det * b2Dot(ex, b2Cross(b, ez)); + x.z = det * b2Dot(ex, b2Cross(ey, b)); return x; } @@ -42,7 +40,7 @@ b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const /// than computing the inverse in one-shot cases. b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const { - float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y; + float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; float32 det = a11 * a22 - a12 * a21; if (det != 0.0f) { diff --git a/src/modules/physics/box2d/Box2D/Common/b2Math.h b/src/modules/physics/box2d/Box2D/Common/b2Math.h old mode 100644 new mode 100755 index c29f59430..fdcd939de --- a/src/modules/physics/box2d/Box2D/Common/b2Math.h +++ b/src/modules/physics/box2d/Box2D/Common/b2Math.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -57,13 +57,8 @@ inline float32 b2InvSqrt(float32 x) return x; } -#define b2Sqrt(x) sqrtf(x) -#define b2Atan2(y, x) atan2f(y, x) - -inline float32 b2Abs(float32 a) -{ - return a > 0.0f ? a : -a; -} +#define b2Sqrt(x) std::sqrt(x) +#define b2Atan2(y, x) std::atan2(y, x) /// A 2D column vector. struct b2Vec2 @@ -147,6 +142,12 @@ struct b2Vec2 return b2IsValid(x) && b2IsValid(y); } + /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other) + b2Vec2 Skew() const + { + return b2Vec2(-y, x); + } + float32 x, y; }; @@ -198,75 +199,49 @@ struct b2Mat22 /// Construct this matrix using columns. b2Mat22(const b2Vec2& c1, const b2Vec2& c2) { - col1 = c1; - col2 = c2; + ex = c1; + ey = c2; } /// Construct this matrix using scalars. b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22) { - col1.x = a11; col1.y = a21; - col2.x = a12; col2.y = a22; - } - - /// Construct this matrix using an angle. This matrix becomes - /// an orthonormal rotation matrix. - explicit b2Mat22(float32 angle) - { - // TODO_ERIN compute sin+cos together. - float32 c = cosf(angle), s = sinf(angle); - col1.x = c; col2.x = -s; - col1.y = s; col2.y = c; + ex.x = a11; ex.y = a21; + ey.x = a12; ey.y = a22; } /// Initialize this matrix using columns. void Set(const b2Vec2& c1, const b2Vec2& c2) { - col1 = c1; - col2 = c2; - } - - /// Initialize this matrix using an angle. This matrix becomes - /// an orthonormal rotation matrix. - void Set(float32 angle) - { - float32 c = cosf(angle), s = sinf(angle); - col1.x = c; col2.x = -s; - col1.y = s; col2.y = c; + ex = c1; + ey = c2; } /// Set this to the identity matrix. void SetIdentity() { - col1.x = 1.0f; col2.x = 0.0f; - col1.y = 0.0f; col2.y = 1.0f; + ex.x = 1.0f; ey.x = 0.0f; + ex.y = 0.0f; ey.y = 1.0f; } /// Set this matrix to all zeros. void SetZero() { - col1.x = 0.0f; col2.x = 0.0f; - col1.y = 0.0f; col2.y = 0.0f; - } - - /// Extract the angle from this matrix (assumed to be - /// a rotation matrix). - float32 GetAngle() const - { - return b2Atan2(col1.y, col1.x); + ex.x = 0.0f; ey.x = 0.0f; + ex.y = 0.0f; ey.y = 0.0f; } b2Mat22 GetInverse() const { - float32 a = col1.x, b = col2.x, c = col1.y, d = col2.y; + float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; b2Mat22 B; float32 det = a * d - b * c; if (det != 0.0f) { det = 1.0f / det; } - B.col1.x = det * d; B.col2.x = -det * b; - B.col1.y = -det * c; B.col2.y = det * a; + B.ex.x = det * d; B.ey.x = -det * b; + B.ex.y = -det * c; B.ey.y = det * a; return B; } @@ -274,7 +249,7 @@ struct b2Mat22 /// than computing the inverse in one-shot cases. b2Vec2 Solve(const b2Vec2& b) const { - float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y; + float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; float32 det = a11 * a22 - a12 * a21; if (det != 0.0f) { @@ -286,7 +261,7 @@ struct b2Mat22 return x; } - b2Vec2 col1, col2; + b2Vec2 ex, ey; }; /// A 3-by-3 matrix. Stored in column-major order. @@ -298,17 +273,17 @@ struct b2Mat33 /// Construct this matrix using columns. b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3) { - col1 = c1; - col2 = c2; - col3 = c3; + ex = c1; + ey = c2; + ez = c3; } /// Set this matrix to all zeros. void SetZero() { - col1.SetZero(); - col2.SetZero(); - col3.SetZero(); + ex.SetZero(); + ey.SetZero(); + ez.SetZero(); } /// Solve A * x = b, where b is a column vector. This is more efficient @@ -320,41 +295,85 @@ struct b2Mat33 /// 2-by-2 matrix equation. b2Vec2 Solve22(const b2Vec2& b) const; - b2Vec3 col1, col2, col3; + b2Vec3 ex, ey, ez; +}; + +/// Rotation +struct b2Rot +{ + b2Rot() {} + + /// Initialize from an angle in radians + explicit b2Rot(float32 angle) + { + /// TODO_ERIN optimize + s = sinf(angle); + c = cosf(angle); + } + + /// Set using an angle in radians. + void Set(float32 angle) + { + /// TODO_ERIN optimize + s = sinf(angle); + c = cosf(angle); + } + + /// Set to the identity rotation + void SetIdentity() + { + s = 0.0f; + c = 1.0f; + } + + /// Get the angle in radians + float32 GetAngle() const + { + return b2Atan2(s, c); + } + + /// Get the x-axis + b2Vec2 GetXAxis() const + { + return b2Vec2(c, s); + } + + /// Get the u-axis + b2Vec2 GetYAxis() const + { + return b2Vec2(-s, c); + } + + /// Sine and cosine + float32 s, c; }; /// A transform contains translation and rotation. It is used to represent /// the position and orientation of rigid frames. struct b2Transform { - /// The default constructor does nothing (for performance). + /// The default constructor does nothing. b2Transform() {} - /// Initialize using a position vector and a rotation matrix. - b2Transform(const b2Vec2& position, const b2Mat22& R) : position(position), R(R) {} + /// Initialize using a position vector and a rotation. + b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {} /// Set this to the identity transform. void SetIdentity() { - position.SetZero(); - R.SetIdentity(); + p.SetZero(); + q.SetIdentity(); } /// Set this based on the position and angle. - void Set(const b2Vec2& p, float32 angle) + void Set(const b2Vec2& position, float32 angle) { - position = p; - R.Set(angle); + p = position; + q.Set(angle); } - /// Calculate the angle that the rotation matrix represents. - float32 GetAngle() const - { - return b2Atan2(R.col1.y, R.col1.x); - } - - b2Vec2 position; - b2Mat22 R; + b2Vec2 p; + b2Rot q; }; /// This describes the motion of a body/shape for TOI computation. @@ -364,12 +383,12 @@ struct b2Transform struct b2Sweep { /// Get the interpolated transform at a specific time. - /// @param alpha is a factor in [0,1], where 0 indicates t0. - void GetTransform(b2Transform* xf, float32 alpha) const; + /// @param beta is a factor in [0,1], where 0 indicates alpha0. + void GetTransform(b2Transform* xfb, float32 beta) const; /// Advance the sweep forward, yielding a new initial state. - /// @param t the new initial time. - void Advance(float32 t); + /// @param alpha the new initial time. + void Advance(float32 alpha); /// Normalize the angles. void Normalize(); @@ -377,12 +396,14 @@ struct b2Sweep b2Vec2 localCenter; ///< local center of mass position b2Vec2 c0, c; ///< center world positions float32 a0, a; ///< world angles + + /// Fraction of the current time step in the range [0,1] + /// c0 and a0 are the positions at alpha0. + float32 alpha0; }; - +/// Useful constant extern const b2Vec2 b2Vec2_zero; -extern const b2Mat22 b2Mat22_identity; -extern const b2Transform b2Transform_identity; /// Perform the dot product on two vectors. inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b) @@ -414,14 +435,14 @@ inline b2Vec2 b2Cross(float32 s, const b2Vec2& a) /// then this transforms the vector from one frame to another. inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v) { - return b2Vec2(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.y * v.y); + return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); } /// Multiply a matrix transpose times a vector. If a rotation matrix is provided, /// then this transforms the vector from one frame to another (inverse transform). inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v) { - return b2Vec2(b2Dot(v, A.col1), b2Dot(v, A.col2)); + return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey)); } /// Add two vectors component-wise. @@ -489,40 +510,109 @@ inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b) inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B) { - return b2Mat22(A.col1 + B.col1, A.col2 + B.col2); + return b2Mat22(A.ex + B.ex, A.ey + B.ey); } // A * B inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B) { - return b2Mat22(b2Mul(A, B.col1), b2Mul(A, B.col2)); + return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey)); } // A^T * B inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B) { - b2Vec2 c1(b2Dot(A.col1, B.col1), b2Dot(A.col2, B.col1)); - b2Vec2 c2(b2Dot(A.col1, B.col2), b2Dot(A.col2, B.col2)); + b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex)); + b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey)); return b2Mat22(c1, c2); } /// Multiply a matrix times a vector. inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v) { - return v.x * A.col1 + v.y * A.col2 + v.z * A.col3; + return v.x * A.ex + v.y * A.ey + v.z * A.ez; +} + +/// Multiply two rotations: q * r +inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r) +{ + // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc] + // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc] + // s = qs * rc + qc * rs + // c = qc * rc - qs * rs + b2Rot qr; + qr.s = q.s * r.c + q.c * r.s; + qr.c = q.c * r.c - q.s * r.s; + return qr; +} + +/// Transpose multiply two rotations: qT * r +inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r) +{ + // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] + // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc] + // s = qc * rs - qs * rc + // c = qc * rc + qs * rs + b2Rot qr; + qr.s = q.c * r.s - q.s * r.c; + qr.c = q.c * r.c + q.s * r.s; + return qr; +} + +/// Rotate a vector +inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v) +{ + return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y); +} + +/// Inverse rotate a vector +inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v) +{ + return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y); } inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v) { - float32 x = T.position.x + T.R.col1.x * v.x + T.R.col2.x * v.y; - float32 y = T.position.y + T.R.col1.y * v.x + T.R.col2.y * v.y; + float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x; + float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y; return b2Vec2(x, y); } inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v) { - return b2MulT(T.R, v - T.position); + float32 px = v.x - T.p.x; + float32 py = v.y - T.p.y; + float32 x = (T.q.c * px + T.q.s * py); + float32 y = (-T.q.s * px + T.q.c * py); + + return b2Vec2(x, y); +} + +// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p +// = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p +inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B) +{ + b2Transform C; + C.q = b2Mul(A.q, B.q); + C.p = b2Mul(A.q, B.p) + A.p; + return C; +} + +// v2 = A.q' * (B.q * v1 + B.p - A.p) +// = A.q' * B.q * v1 + A.q' * (B.p - A.p) +inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B) +{ + b2Transform C; + C.q = b2MulT(A.q, B.q); + C.p = b2MulT(A.q, B.p - A.p); + return C; +} + +template +inline T b2Abs(T a) +{ + return a > T(0) ? a : -a; } inline b2Vec2 b2Abs(const b2Vec2& a) @@ -532,7 +622,7 @@ inline b2Vec2 b2Abs(const b2Vec2& a) inline b2Mat22 b2Abs(const b2Mat22& A) { - return b2Mat22(b2Abs(A.col1), b2Abs(A.col2)); + return b2Mat22(b2Abs(A.ex), b2Abs(A.ey)); } template @@ -596,20 +686,23 @@ inline bool b2IsPowerOfTwo(uint32 x) return result; } -inline void b2Sweep::GetTransform(b2Transform* xf, float32 alpha) const +inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const { - xf->position = (1.0f - alpha) * c0 + alpha * c; - float32 angle = (1.0f - alpha) * a0 + alpha * a; - xf->R.Set(angle); + xf->p = (1.0f - beta) * c0 + beta * c; + float32 angle = (1.0f - beta) * a0 + beta * a; + xf->q.Set(angle); // Shift to origin - xf->position -= b2Mul(xf->R, localCenter); + xf->p -= b2Mul(xf->q, localCenter); } -inline void b2Sweep::Advance(float32 t) +inline void b2Sweep::Advance(float32 alpha) { - c0 = (1.0f - t) * c0 + t * c; - a0 = (1.0f - t) * a0 + t * a; + b2Assert(alpha0 < 1.0f); + float32 beta = (alpha - alpha0) / (1.0f - alpha0); + c0 = (1.0f - beta) * c0 + beta * c; + a0 = (1.0f - beta) * a0 + beta * a; + alpha0 = alpha; } /// Normalize an angle in radians to be between -pi and pi diff --git a/src/modules/physics/box2d/Box2D/Common/b2Settings.cpp b/src/modules/physics/box2d/Box2D/Common/b2Settings.cpp old mode 100644 new mode 100755 index da308149f..97f5283f4 --- a/src/modules/physics/box2d/Box2D/Common/b2Settings.cpp +++ b/src/modules/physics/box2d/Box2D/Common/b2Settings.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -19,7 +19,7 @@ #include #include -b2Version b2_version = {2, 1, 2}; +b2Version b2_version = {2, 2, 0}; // Memory allocators. Modify these to use your own allocator. void* b2Alloc(int32 size) diff --git a/src/modules/physics/box2d/Box2D/Common/b2Settings.h b/src/modules/physics/box2d/Box2D/Common/b2Settings.h old mode 100644 new mode 100755 index 1aa8c2661..eabc526d0 --- a/src/modules/physics/box2d/Box2D/Common/b2Settings.h +++ b/src/modules/physics/box2d/Box2D/Common/b2Settings.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -32,6 +32,7 @@ typedef unsigned char uint8; typedef unsigned short uint16; typedef unsigned int uint32; typedef float float32; +typedef double float64; #define b2_maxFloat FLT_MAX #define b2_epsilon FLT_EPSILON @@ -43,10 +44,12 @@ typedef float float32; // Collision -/// The maximum number of contact points between two convex shapes. +/// The maximum number of contact points between two convex shapes. Do +/// not change this value. #define b2_maxManifoldPoints 2 -/// The maximum number of vertices on a convex polygon. +/// The maximum number of vertices on a convex polygon. You cannot increase +/// this too much because b2BlockAllocator has a maximum object size. #define b2_maxPolygonVertices 8 /// This is used to fatten AABBs in the dynamic tree. This allows proxies @@ -72,6 +75,9 @@ typedef float float32; /// Making it larger may create artifacts for vertex collision. #define b2_polygonRadius (2.0f * b2_linearSlop) +/// Maximum number of sub-steps per contact in continuous physics simulation. +#define b2_maxSubSteps 8 + // Dynamics @@ -103,7 +109,9 @@ typedef float float32; /// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so /// that overlap is removed in one time step. However using values close to 1 often lead /// to overshoot. -#define b2_contactBaumgarte 0.2f +#define b2_baumgarte 0.2f +#define b2_toiBaugarte 0.75f + // Sleep @@ -136,16 +144,4 @@ struct b2Version /// Current version. extern b2Version b2_version; -/// Friction mixing law. Feel free to customize this. -inline float32 b2MixFriction(float32 friction1, float32 friction2) -{ - return sqrtf(friction1 * friction2); -} - -/// Restitution mixing law. Feel free to customize this. -inline float32 b2MixRestitution(float32 restitution1, float32 restitution2) -{ - return restitution1 > restitution2 ? restitution1 : restitution2; -} - #endif diff --git a/src/modules/physics/box2d/Box2D/Common/b2StackAllocator.cpp b/src/modules/physics/box2d/Box2D/Common/b2StackAllocator.cpp old mode 100644 new mode 100755 index cb2c42e01..4a862835b --- a/src/modules/physics/box2d/Box2D/Common/b2StackAllocator.cpp +++ b/src/modules/physics/box2d/Box2D/Common/b2StackAllocator.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages diff --git a/src/modules/physics/box2d/Box2D/Common/b2StackAllocator.h b/src/modules/physics/box2d/Box2D/Common/b2StackAllocator.h old mode 100644 new mode 100755 index 315cc02ff..796c51dfd --- a/src/modules/physics/box2d/Box2D/Common/b2StackAllocator.h +++ b/src/modules/physics/box2d/Box2D/Common/b2StackAllocator.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages diff --git a/src/modules/physics/box2d/Box2D/Common/b2Timer.cpp b/src/modules/physics/box2d/Box2D/Common/b2Timer.cpp new file mode 100755 index 000000000..b1863c296 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Common/b2Timer.cpp @@ -0,0 +1,100 @@ +/* +* Copyright (c) 2011 Erin Catto http://box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include + +#if defined(WIN32) + +float64 b2Timer::s_invFrequency = 0.0f; + +#include + +b2Timer::b2Timer() +{ + LARGE_INTEGER largeInteger; + + if (s_invFrequency == 0.0f) + { + QueryPerformanceFrequency(&largeInteger); + s_invFrequency = float64(largeInteger.QuadPart); + if (s_invFrequency > 0.0f) + { + s_invFrequency = 1000.0f / s_invFrequency; + } + } + + QueryPerformanceCounter(&largeInteger); + m_start = float64(largeInteger.QuadPart); +} + +void b2Timer::Reset() +{ + LARGE_INTEGER largeInteger; + QueryPerformanceCounter(&largeInteger); + m_start = float64(largeInteger.QuadPart); +} + +float32 b2Timer::GetMilliseconds() const +{ + LARGE_INTEGER largeInteger; + QueryPerformanceCounter(&largeInteger); + float64 count = float64(largeInteger.QuadPart); + float32 ms = float32(s_invFrequency * (count - m_start)); + return ms; +} + +#elif defined(__linux__) || defined (__APPLE__) + +#include + +b2Timer::b2Timer() +{ + Reset(); +} + +void b2Timer::Reset() +{ + timeval t; + gettimeofday(&t, 0); + m_start_sec = t.tv_sec; + m_start_msec = t.tv_usec * 0.001f; +} + +float32 b2Timer::GetMilliseconds() const +{ + timeval t; + gettimeofday(&t, 0); + return (t.tv_sec - m_start_sec) * 1000 + t.tv_usec * 0.001f - m_start_msec; +} + +#else + +b2Timer::b2Timer() +{ +} + +void b2Timer::Reset() +{ +} + +float32 b2Timer::GetMilliseconds() const +{ + return 0.0f; +} + +#endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2TOISolver.h b/src/modules/physics/box2d/Box2D/Common/b2Timer.h old mode 100644 new mode 100755 similarity index 54% rename from src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2TOISolver.h rename to src/modules/physics/box2d/Box2D/Common/b2Timer.h index c092e2a81..678f6ffe4 --- a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2TOISolver.h +++ b/src/modules/physics/box2d/Box2D/Common/b2Timer.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com +* Copyright (c) 2011 Erin Catto http://box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -16,36 +16,30 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef B2_TOI_SOLVER_H -#define B2_TOI_SOLVER_H +#include -#include - -class b2Contact; -class b2Body; -struct b2TOIConstraint; -class b2StackAllocator; - -/// This is a pure position solver for a single movable body in contact with -/// multiple non-moving bodies. -class b2TOISolver +/// Timer for profiling. This has platform specific code and may +/// not work on every platform. +class b2Timer { public: - b2TOISolver(b2StackAllocator* allocator); - ~b2TOISolver(); - void Initialize(b2Contact** contacts, int32 contactCount, b2Body* toiBody); - void Clear(); + /// Constructor + b2Timer(); - // Perform one solver iteration. Returns true if converged. - bool Solve(float32 baumgarte); + /// Reset the timer. + void Reset(); + + /// Get the time since construction or the last reset. + float32 GetMilliseconds() const; private: - b2TOIConstraint* m_constraints; - int32 m_count; - b2Body* m_toiBody; - b2StackAllocator* m_allocator; -}; - +#if defined(WIN32) + float64 m_start; + static float64 s_invFrequency; +#elif defined(__linux__) || defined (__APPLE__) + unsigned long m_start_sec; + unsigned long m_start_msec; #endif +}; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp new file mode 100755 index 000000000..3886dfd70 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp @@ -0,0 +1,54 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include + +#include +using namespace std; + +b2Contact* b2ChainAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2ChainAndCircleContact)); + return new (mem) b2ChainAndCircleContact(fixtureA, indexA, fixtureB, indexB); +} + +void b2ChainAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2ChainAndCircleContact*)contact)->~b2ChainAndCircleContact(); + allocator->Free(contact, sizeof(b2ChainAndCircleContact)); +} + +b2ChainAndCircleContact::b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB) +: b2Contact(fixtureA, indexA, fixtureB, indexB) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_chain); + b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); +} + +void b2ChainAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape(); + b2EdgeShape edge; + chain->GetChildEdge(&edge, m_indexA); + b2CollideEdgeAndCircle( manifold, &edge, xfA, + (b2CircleShape*)m_fixtureB->GetShape(), xfB); +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h new file mode 100755 index 000000000..2dad0b64f --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h @@ -0,0 +1,39 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B2_CHAIN_AND_CIRCLE_CONTACT_H +#define B2_CHAIN_AND_CIRCLE_CONTACT_H + +#include + +class b2BlockAllocator; + +class b2ChainAndCircleContact : public b2Contact +{ +public: + static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, + b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); + static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); + + b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); + ~b2ChainAndCircleContact() {} + + void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); +}; + +#endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp new file mode 100755 index 000000000..02bcaaf6a --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp @@ -0,0 +1,54 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include + +#include +using namespace std; + +b2Contact* b2ChainAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2ChainAndPolygonContact)); + return new (mem) b2ChainAndPolygonContact(fixtureA, indexA, fixtureB, indexB); +} + +void b2ChainAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2ChainAndPolygonContact*)contact)->~b2ChainAndPolygonContact(); + allocator->Free(contact, sizeof(b2ChainAndPolygonContact)); +} + +b2ChainAndPolygonContact::b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB) +: b2Contact(fixtureA, indexA, fixtureB, indexB) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_chain); + b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); +} + +void b2ChainAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape(); + b2EdgeShape edge; + chain->GetChildEdge(&edge, m_indexA); + b2CollideEdgeAndPolygon( manifold, &edge, xfA, + (b2PolygonShape*)m_fixtureB->GetShape(), xfB); +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h new file mode 100755 index 000000000..8f30ee867 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h @@ -0,0 +1,39 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B2_CHAIN_AND_POLYGON_CONTACT_H +#define B2_CHAIN_AND_POLYGON_CONTACT_H + +#include + +class b2BlockAllocator; + +class b2ChainAndPolygonContact : public b2Contact +{ +public: + static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, + b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); + static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); + + b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); + ~b2ChainAndPolygonContact() {} + + void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); +}; + +#endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2CircleContact.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2CircleContact.cpp old mode 100644 new mode 100755 index 6df009125..584ef2f1f --- a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2CircleContact.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2CircleContact.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -24,8 +24,9 @@ #include #include +using namespace std; -b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator) +b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) { void* mem = allocator->Allocate(sizeof(b2CircleContact)); return new (mem) b2CircleContact(fixtureA, fixtureB); @@ -38,7 +39,7 @@ void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) } b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) - : b2Contact(fixtureA, fixtureB) + : b2Contact(fixtureA, 0, fixtureB, 0) { b2Assert(m_fixtureA->GetType() == b2Shape::e_circle); b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2CircleContact.h b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2CircleContact.h old mode 100644 new mode 100755 index afc277aa0..aac1f0bd6 --- a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2CircleContact.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2CircleContact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -26,7 +26,8 @@ class b2BlockAllocator; class b2CircleContact : public b2Contact { public: - static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator); + static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, + b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2Contact.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2Contact.cpp old mode 100644 new mode 100755 index e7ad1ca5b..557af7f08 --- a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2Contact.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2Contact.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -20,6 +20,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -38,13 +42,17 @@ void b2Contact::InitializeRegisters() AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle); AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle); AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon); + AddType(b2EdgeAndCircleContact::Create, b2EdgeAndCircleContact::Destroy, b2Shape::e_edge, b2Shape::e_circle); + AddType(b2EdgeAndPolygonContact::Create, b2EdgeAndPolygonContact::Destroy, b2Shape::e_edge, b2Shape::e_polygon); + AddType(b2ChainAndCircleContact::Create, b2ChainAndCircleContact::Destroy, b2Shape::e_chain, b2Shape::e_circle); + AddType(b2ChainAndPolygonContact::Create, b2ChainAndPolygonContact::Destroy, b2Shape::e_chain, b2Shape::e_polygon); } void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn, b2Shape::Type type1, b2Shape::Type type2) { - b2Assert(b2Shape::e_unknown < type1 && type1 < b2Shape::e_typeCount); - b2Assert(b2Shape::e_unknown < type2 && type2 < b2Shape::e_typeCount); + b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount); + b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount); s_registers[type1][type2].createFcn = createFcn; s_registers[type1][type2].destroyFcn = destoryFcn; @@ -58,7 +66,7 @@ void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* dest } } -b2Contact* b2Contact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator) +b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) { if (s_initialized == false) { @@ -69,19 +77,19 @@ b2Contact* b2Contact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAl b2Shape::Type type1 = fixtureA->GetType(); b2Shape::Type type2 = fixtureB->GetType(); - b2Assert(b2Shape::e_unknown < type1 && type1 < b2Shape::e_typeCount); - b2Assert(b2Shape::e_unknown < type2 && type2 < b2Shape::e_typeCount); + b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount); + b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount); b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn; if (createFcn) { if (s_registers[type1][type2].primary) { - return createFcn(fixtureA, fixtureB, allocator); + return createFcn(fixtureA, indexA, fixtureB, indexB, allocator); } else { - return createFcn(fixtureB, fixtureA, allocator); + return createFcn(fixtureB, indexB, fixtureA, indexA, allocator); } } else @@ -103,20 +111,23 @@ void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) b2Shape::Type typeA = contact->GetFixtureA()->GetType(); b2Shape::Type typeB = contact->GetFixtureB()->GetType(); - b2Assert(b2Shape::e_unknown < typeA && typeB < b2Shape::e_typeCount); - b2Assert(b2Shape::e_unknown < typeA && typeB < b2Shape::e_typeCount); + b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount); + b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount); b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn; destroyFcn(contact, allocator); } -b2Contact::b2Contact(b2Fixture* fA, b2Fixture* fB) +b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB) { m_flags = e_enabledFlag; m_fixtureA = fA; m_fixtureB = fB; + m_indexA = indexA; + m_indexB = indexB; + m_manifold.pointCount = 0; m_prev = NULL; @@ -133,6 +144,9 @@ b2Contact::b2Contact(b2Fixture* fA, b2Fixture* fB) m_nodeB.other = NULL; m_toiCount = 0; + + m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction); + m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); } // Update the contact manifold and touching status. @@ -161,7 +175,7 @@ void b2Contact::Update(b2ContactListener* listener) { const b2Shape* shapeA = m_fixtureA->GetShape(); const b2Shape* shapeB = m_fixtureB->GetShape(); - touching = b2TestOverlap(shapeA, shapeB, xfA, xfB); + touching = b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB); // Sensors don't generate manifolds. m_manifold.pointCount = 0; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2Contact.h b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2Contact.h old mode 100644 new mode 100755 index a6add3dc4..7b5a6ab88 --- a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2Contact.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2Contact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -22,7 +22,6 @@ #include #include #include -#include #include class b2Body; @@ -33,7 +32,23 @@ class b2BlockAllocator; class b2StackAllocator; class b2ContactListener; -typedef b2Contact* b2ContactCreateFcn(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator); +/// Friction mixing law. The idea is to allow either fixture to drive the restitution to zero. +/// For example, anything slides on ice. +inline float32 b2MixFriction(float32 friction1, float32 friction2) +{ + return std::sqrt(friction1 * friction2); +} + +/// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface. +/// For example, a superball bounces on anything. +inline float32 b2MixRestitution(float32 restitution1, float32 restitution2) +{ + return restitution1 > restitution2 ? restitution1 : restitution2; +} + +typedef b2Contact* b2ContactCreateFcn( b2Fixture* fixtureA, int32 indexA, + b2Fixture* fixtureB, int32 indexB, + b2BlockAllocator* allocator); typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator); struct b2ContactRegister @@ -86,14 +101,40 @@ public: b2Contact* GetNext(); const b2Contact* GetNext() const; - /// Get the first fixture in this contact. + /// Get fixture A in this contact. b2Fixture* GetFixtureA(); const b2Fixture* GetFixtureA() const; - /// Get the second fixture in this contact. + /// Get the child primitive index for fixture A. + int32 GetChildIndexA() const; + + /// Get fixture B in this contact. b2Fixture* GetFixtureB(); const b2Fixture* GetFixtureB() const; + /// Get the child primitive index for fixture B. + int32 GetChildIndexB() const; + + /// Override the default friction mixture. You can call this in b2ContactListener::PreSolve. + /// This value persists until set or reset. + void SetFriction(float32 friction); + + /// Get the friction. + float32 GetFriction() const; + + /// Reset the friction mixture to the default value. + void ResetFriction(); + + /// Override the default restitution mixture. You can call this in b2ContactListener::PreSolve. + /// The value persists until you set or reset. + void SetRestitution(float32 restitution); + + /// Get the restitution. + float32 GetRestitution() const; + + /// Reset the restitution to the default value. + void ResetRestitution(); + /// Evaluate this contact with your own manifold and transforms. virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0; @@ -122,6 +163,8 @@ protected: // This bullet contact had a TOI event e_bulletHitFlag = 0x0010, + // This contact has a valid TOI in m_toi + e_toiFlag = 0x0020 }; /// Flag this contact for filtering. Filtering will occur the next time step. @@ -130,12 +173,12 @@ protected: static void AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destroyFcn, b2Shape::Type typeA, b2Shape::Type typeB); static void InitializeRegisters(); - static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator); + static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); static void Destroy(b2Contact* contact, b2Shape::Type typeA, b2Shape::Type typeB, b2BlockAllocator* allocator); static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); b2Contact() : m_fixtureA(NULL), m_fixtureB(NULL) {} - b2Contact(b2Fixture* fixtureA, b2Fixture* fixtureB); + b2Contact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); virtual ~b2Contact() {} void Update(b2ContactListener* listener); @@ -156,10 +199,16 @@ protected: b2Fixture* m_fixtureA; b2Fixture* m_fixtureB; + int32 m_indexA; + int32 m_indexB; + b2Manifold m_manifold; int32 m_toiCount; -// float32 m_toi; + float32 m_toi; + + float32 m_friction; + float32 m_restitution; }; inline b2Manifold* b2Contact::GetManifold() @@ -229,14 +278,54 @@ inline b2Fixture* b2Contact::GetFixtureB() return m_fixtureB; } +inline int32 b2Contact::GetChildIndexA() const +{ + return m_indexA; +} + inline const b2Fixture* b2Contact::GetFixtureB() const { return m_fixtureB; } +inline int32 b2Contact::GetChildIndexB() const +{ + return m_indexB; +} + inline void b2Contact::FlagForFiltering() { m_flags |= e_filterFlag; } +inline void b2Contact::SetFriction(float32 friction) +{ + m_friction = friction; +} + +inline float32 b2Contact::GetFriction() const +{ + return m_friction; +} + +inline void b2Contact::ResetFriction() +{ + m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction); +} + +inline void b2Contact::SetRestitution(float32 restitution) +{ + m_restitution = restitution; +} + +inline float32 b2Contact::GetRestitution() const +{ + return m_restitution; +} + +inline void b2Contact::ResetRestitution() +{ + m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); +} + #endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.cpp old mode 100644 new mode 100755 index f65fb60ee..c990d7fb8 --- a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -17,6 +17,7 @@ */ #include + #include #include #include @@ -25,17 +26,36 @@ #define B2_DEBUG_SOLVER 0 -b2ContactSolver::b2ContactSolver(b2Contact** contacts, int32 contactCount, - b2StackAllocator* allocator, float32 impulseRatio) +struct b2ContactPositionConstraint { - m_allocator = allocator; + b2Vec2 localPoints[b2_maxManifoldPoints]; + b2Vec2 localNormal; + b2Vec2 localPoint; + int32 indexA; + int32 indexB; + float32 invMassA, invMassB; + b2Vec2 localCenterA, localCenterB; + float32 invIA, invIB; + b2Manifold::Type type; + float32 radiusA, radiusB; + int32 pointCount; +}; - m_constraintCount = contactCount; - m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_constraintCount * sizeof(b2ContactConstraint)); +b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def) +{ + m_step = def->step; + m_allocator = def->allocator; + m_count = def->count; + m_positionConstraints = (b2ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactPositionConstraint)); + m_velocityConstraints = (b2ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactVelocityConstraint)); + m_positions = def->positions; + m_velocities = def->velocities; + m_contacts = def->contacts; - for (int32 i = 0; i < m_constraintCount; ++i) + // Initialize position independent portions of the constraints. + for (int32 i = 0; i < m_count; ++i) { - b2Contact* contact = contacts[i]; + b2Contact* contact = m_contacts[i]; b2Fixture* fixtureA = contact->m_fixtureA; b2Fixture* fixtureB = contact->m_fixtureB; @@ -47,222 +67,302 @@ b2ContactSolver::b2ContactSolver(b2Contact** contacts, int32 contactCount, b2Body* bodyB = fixtureB->GetBody(); b2Manifold* manifold = contact->GetManifold(); - float32 friction = b2MixFriction(fixtureA->GetFriction(), fixtureB->GetFriction()); - float32 restitution = b2MixRestitution(fixtureA->GetRestitution(), fixtureB->GetRestitution()); + int32 pointCount = manifold->pointCount; + b2Assert(pointCount > 0); - b2Vec2 vA = bodyA->m_linearVelocity; - b2Vec2 vB = bodyB->m_linearVelocity; - float32 wA = bodyA->m_angularVelocity; - float32 wB = bodyB->m_angularVelocity; + b2ContactVelocityConstraint* vc = m_velocityConstraints + i; + vc->friction = contact->m_friction; + vc->restitution = contact->m_restitution; + vc->indexA = bodyA->m_islandIndex; + vc->indexB = bodyB->m_islandIndex; + vc->invMassA = bodyA->m_invMass; + vc->invMassB = bodyB->m_invMass; + vc->invIA = bodyA->m_invI; + vc->invIB = bodyB->m_invI; + vc->contactIndex = i; + vc->pointCount = pointCount; + vc->K.SetZero(); + vc->normalMass.SetZero(); - b2Assert(manifold->pointCount > 0); + b2ContactPositionConstraint* pc = m_positionConstraints + i; + pc->indexA = bodyA->m_islandIndex; + pc->indexB = bodyB->m_islandIndex; + pc->invMassA = bodyA->m_invMass; + pc->invMassB = bodyB->m_invMass; + pc->localCenterA = bodyA->m_sweep.localCenter; + pc->localCenterB = bodyB->m_sweep.localCenter; + pc->invIA = bodyA->m_invI; + pc->invIB = bodyB->m_invI; + pc->localNormal = manifold->localNormal; + pc->localPoint = manifold->localPoint; + pc->pointCount = pointCount; + pc->radiusA = radiusA; + pc->radiusB = radiusB; + pc->type = manifold->type; - b2WorldManifold worldManifold; - worldManifold.Initialize(manifold, bodyA->m_xf, radiusA, bodyB->m_xf, radiusB); - - b2ContactConstraint* cc = m_constraints + i; - cc->bodyA = bodyA; - cc->bodyB = bodyB; - cc->manifold = manifold; - cc->normal = worldManifold.normal; - cc->pointCount = manifold->pointCount; - cc->friction = friction; - - cc->localNormal = manifold->localNormal; - cc->localPoint = manifold->localPoint; - cc->radius = radiusA + radiusB; - cc->type = manifold->type; - - for (int32 j = 0; j < cc->pointCount; ++j) + for (int32 j = 0; j < pointCount; ++j) { b2ManifoldPoint* cp = manifold->points + j; - b2ContactConstraintPoint* ccp = cc->points + j; - - ccp->normalImpulse = impulseRatio * cp->normalImpulse; - ccp->tangentImpulse = impulseRatio * cp->tangentImpulse; - - ccp->localPoint = cp->localPoint; - - ccp->rA = worldManifold.points[j] - bodyA->m_sweep.c; - ccp->rB = worldManifold.points[j] - bodyB->m_sweep.c; - - float32 rnA = b2Cross(ccp->rA, cc->normal); - float32 rnB = b2Cross(ccp->rB, cc->normal); - rnA *= rnA; - rnB *= rnB; - - float32 kNormal = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rnA + bodyB->m_invI * rnB; - - b2Assert(kNormal > b2_epsilon); - ccp->normalMass = 1.0f / kNormal; - - b2Vec2 tangent = b2Cross(cc->normal, 1.0f); - - float32 rtA = b2Cross(ccp->rA, tangent); - float32 rtB = b2Cross(ccp->rB, tangent); - rtA *= rtA; - rtB *= rtB; - - float32 kTangent = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rtA + bodyB->m_invI * rtB; - - b2Assert(kTangent > b2_epsilon); - ccp->tangentMass = 1.0f / kTangent; - - // Setup a velocity bias for restitution. - ccp->velocityBias = 0.0f; - float32 vRel = b2Dot(cc->normal, vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA)); - if (vRel < -b2_velocityThreshold) + b2VelocityConstraintPoint* vcp = vc->points + j; + + if (m_step.warmStarting) { - ccp->velocityBias = -restitution * vRel; - } - } - - // If we have two points, then prepare the block solver. - if (cc->pointCount == 2) - { - b2ContactConstraintPoint* ccp1 = cc->points + 0; - b2ContactConstraintPoint* ccp2 = cc->points + 1; - - float32 invMassA = bodyA->m_invMass; - float32 invIA = bodyA->m_invI; - float32 invMassB = bodyB->m_invMass; - float32 invIB = bodyB->m_invI; - - float32 rn1A = b2Cross(ccp1->rA, cc->normal); - float32 rn1B = b2Cross(ccp1->rB, cc->normal); - float32 rn2A = b2Cross(ccp2->rA, cc->normal); - float32 rn2B = b2Cross(ccp2->rB, cc->normal); - - float32 k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; - float32 k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; - float32 k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; - - // Ensure a reasonable condition number. - const float32 k_maxConditionNumber = 100.0f; - if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) - { - // K is safe to invert. - cc->K.col1.Set(k11, k12); - cc->K.col2.Set(k12, k22); - cc->normalMass = cc->K.GetInverse(); + vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse; + vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse; } else { - // The constraints are redundant, just use one. - // TODO_ERIN use deepest? - cc->pointCount = 1; + vcp->normalImpulse = 0.0f; + vcp->tangentImpulse = 0.0f; } + + vcp->rA.SetZero(); + vcp->rB.SetZero(); + vcp->normalMass = 0.0f; + vcp->tangentMass = 0.0f; + vcp->velocityBias = 0.0f; + + pc->localPoints[j] = cp->localPoint; } } } b2ContactSolver::~b2ContactSolver() { - m_allocator->Free(m_constraints); + m_allocator->Free(m_velocityConstraints); + m_allocator->Free(m_positionConstraints); +} + +// Initialize position dependent portions of the velocity constraints. +void b2ContactSolver::InitializeVelocityConstraints() +{ + for (int32 i = 0; i < m_count; ++i) + { + b2ContactVelocityConstraint* vc = m_velocityConstraints + i; + b2ContactPositionConstraint* pc = m_positionConstraints + i; + + float32 radiusA = pc->radiusA; + float32 radiusB = pc->radiusB; + b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold(); + + int32 indexA = vc->indexA; + int32 indexB = vc->indexB; + + float32 mA = vc->invMassA; + float32 mB = vc->invMassB; + float32 iA = vc->invIA; + float32 iB = vc->invIB; + b2Vec2 localCenterA = pc->localCenterA; + b2Vec2 localCenterB = pc->localCenterB; + + b2Vec2 cA = m_positions[indexA].c; + float32 aA = m_positions[indexA].a; + b2Vec2 vA = m_velocities[indexA].v; + float32 wA = m_velocities[indexA].w; + + b2Vec2 cB = m_positions[indexB].c; + float32 aB = m_positions[indexB].a; + b2Vec2 vB = m_velocities[indexB].v; + float32 wB = m_velocities[indexB].w; + + b2Assert(manifold->pointCount > 0); + + b2Transform xfA, xfB; + xfA.q.Set(aA); + xfB.q.Set(aB); + xfA.p = cA - b2Mul(xfA.q, localCenterA); + xfB.p = cB - b2Mul(xfB.q, localCenterB); + + b2WorldManifold worldManifold; + worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB); + + vc->normal = worldManifold.normal; + + int32 pointCount = vc->pointCount; + for (int32 j = 0; j < pointCount; ++j) + { + b2VelocityConstraintPoint* vcp = vc->points + j; + + vcp->rA = worldManifold.points[j] - cA; + vcp->rB = worldManifold.points[j] - cB; + + float32 rnA = b2Cross(vcp->rA, vc->normal); + float32 rnB = b2Cross(vcp->rB, vc->normal); + + float32 kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + + vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + b2Vec2 tangent = b2Cross(vc->normal, 1.0f); + + float32 rtA = b2Cross(vcp->rA, tangent); + float32 rtB = b2Cross(vcp->rB, tangent); + + float32 kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; + + vcp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; + + // Setup a velocity bias for restitution. + vcp->velocityBias = 0.0f; + float32 vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA)); + if (vRel < -b2_velocityThreshold) + { + vcp->velocityBias = -vc->restitution * vRel; + } + } + + // If we have two points, then prepare the block solver. + if (vc->pointCount == 2) + { + b2VelocityConstraintPoint* vcp1 = vc->points + 0; + b2VelocityConstraintPoint* vcp2 = vc->points + 1; + + float32 rn1A = b2Cross(vcp1->rA, vc->normal); + float32 rn1B = b2Cross(vcp1->rB, vc->normal); + float32 rn2A = b2Cross(vcp2->rA, vc->normal); + float32 rn2B = b2Cross(vcp2->rB, vc->normal); + + float32 k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; + float32 k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; + float32 k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B; + + // Ensure a reasonable condition number. + const float32 k_maxConditionNumber = 1000.0f; + if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) + { + // K is safe to invert. + vc->K.ex.Set(k11, k12); + vc->K.ey.Set(k12, k22); + vc->normalMass = vc->K.GetInverse(); + } + else + { + // The constraints are redundant, just use one. + // TODO_ERIN use deepest? + vc->pointCount = 1; + } + } + } } void b2ContactSolver::WarmStart() { // Warm start. - for (int32 i = 0; i < m_constraintCount; ++i) + for (int32 i = 0; i < m_count; ++i) { - b2ContactConstraint* c = m_constraints + i; + b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - b2Body* bodyA = c->bodyA; - b2Body* bodyB = c->bodyB; - float32 invMassA = bodyA->m_invMass; - float32 invIA = bodyA->m_invI; - float32 invMassB = bodyB->m_invMass; - float32 invIB = bodyB->m_invI; - b2Vec2 normal = c->normal; + int32 indexA = vc->indexA; + int32 indexB = vc->indexB; + float32 mA = vc->invMassA; + float32 iA = vc->invIA; + float32 mB = vc->invMassB; + float32 iB = vc->invIB; + int32 pointCount = vc->pointCount; + + b2Vec2 vA = m_velocities[indexA].v; + float32 wA = m_velocities[indexA].w; + b2Vec2 vB = m_velocities[indexB].v; + float32 wB = m_velocities[indexB].w; + + b2Vec2 normal = vc->normal; b2Vec2 tangent = b2Cross(normal, 1.0f); - for (int32 j = 0; j < c->pointCount; ++j) + for (int32 j = 0; j < pointCount; ++j) { - b2ContactConstraintPoint* ccp = c->points + j; - b2Vec2 P = ccp->normalImpulse * normal + ccp->tangentImpulse * tangent; - bodyA->m_angularVelocity -= invIA * b2Cross(ccp->rA, P); - bodyA->m_linearVelocity -= invMassA * P; - bodyB->m_angularVelocity += invIB * b2Cross(ccp->rB, P); - bodyB->m_linearVelocity += invMassB * P; + b2VelocityConstraintPoint* vcp = vc->points + j; + b2Vec2 P = vcp->normalImpulse * normal + vcp->tangentImpulse * tangent; + wA -= iA * b2Cross(vcp->rA, P); + vA -= mA * P; + wB += iB * b2Cross(vcp->rB, P); + vB += mB * P; } + + m_velocities[indexA].v = vA; + m_velocities[indexA].w = wA; + m_velocities[indexB].v = vB; + m_velocities[indexB].w = wB; } } void b2ContactSolver::SolveVelocityConstraints() { - for (int32 i = 0; i < m_constraintCount; ++i) + for (int32 i = 0; i < m_count; ++i) { - b2ContactConstraint* c = m_constraints + i; - b2Body* bodyA = c->bodyA; - b2Body* bodyB = c->bodyB; - float32 wA = bodyA->m_angularVelocity; - float32 wB = bodyB->m_angularVelocity; - b2Vec2 vA = bodyA->m_linearVelocity; - b2Vec2 vB = bodyB->m_linearVelocity; - float32 invMassA = bodyA->m_invMass; - float32 invIA = bodyA->m_invI; - float32 invMassB = bodyB->m_invMass; - float32 invIB = bodyB->m_invI; - b2Vec2 normal = c->normal; + b2ContactVelocityConstraint* vc = m_velocityConstraints + i; + + int32 indexA = vc->indexA; + int32 indexB = vc->indexB; + float32 mA = vc->invMassA; + float32 iA = vc->invIA; + float32 mB = vc->invMassB; + float32 iB = vc->invIB; + int32 pointCount = vc->pointCount; + + b2Vec2 vA = m_velocities[indexA].v; + float32 wA = m_velocities[indexA].w; + b2Vec2 vB = m_velocities[indexB].v; + float32 wB = m_velocities[indexB].w; + + b2Vec2 normal = vc->normal; b2Vec2 tangent = b2Cross(normal, 1.0f); - float32 friction = c->friction; + float32 friction = vc->friction; - b2Assert(c->pointCount == 1 || c->pointCount == 2); + b2Assert(pointCount == 1 || pointCount == 2); - // Solve tangent constraints - for (int32 j = 0; j < c->pointCount; ++j) + // Solve tangent constraints first because non-penetration is more important + // than friction. + for (int32 j = 0; j < pointCount; ++j) { - b2ContactConstraintPoint* ccp = c->points + j; + b2VelocityConstraintPoint* vcp = vc->points + j; // Relative velocity at contact - b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA); + b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); // Compute tangent force float32 vt = b2Dot(dv, tangent); - float32 lambda = ccp->tangentMass * (-vt); + float32 lambda = vcp->tangentMass * (-vt); // b2Clamp the accumulated force - float32 maxFriction = friction * ccp->normalImpulse; - float32 newImpulse = b2Clamp(ccp->tangentImpulse + lambda, -maxFriction, maxFriction); - lambda = newImpulse - ccp->tangentImpulse; + float32 maxFriction = friction * vcp->normalImpulse; + float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction); + lambda = newImpulse - vcp->tangentImpulse; + vcp->tangentImpulse = newImpulse; // Apply contact impulse b2Vec2 P = lambda * tangent; - vA -= invMassA * P; - wA -= invIA * b2Cross(ccp->rA, P); + vA -= mA * P; + wA -= iA * b2Cross(vcp->rA, P); - vB += invMassB * P; - wB += invIB * b2Cross(ccp->rB, P); - - ccp->tangentImpulse = newImpulse; + vB += mB * P; + wB += iB * b2Cross(vcp->rB, P); } // Solve normal constraints - if (c->pointCount == 1) + if (vc->pointCount == 1) { - b2ContactConstraintPoint* ccp = c->points + 0; + b2VelocityConstraintPoint* vcp = vc->points + 0; // Relative velocity at contact - b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA); + b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); // Compute normal impulse float32 vn = b2Dot(dv, normal); - float32 lambda = -ccp->normalMass * (vn - ccp->velocityBias); + float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias); // b2Clamp the accumulated impulse - float32 newImpulse = b2Max(ccp->normalImpulse + lambda, 0.0f); - lambda = newImpulse - ccp->normalImpulse; + float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f); + lambda = newImpulse - vcp->normalImpulse; + vcp->normalImpulse = newImpulse; // Apply contact impulse b2Vec2 P = lambda * normal; - vA -= invMassA * P; - wA -= invIA * b2Cross(ccp->rA, P); + vA -= mA * P; + wA -= iA * b2Cross(vcp->rA, P); - vB += invMassB * P; - wB += invIB * b2Cross(ccp->rB, P); - ccp->normalImpulse = newImpulse; + vB += mB * P; + wB += iB * b2Cross(vcp->rB, P); } else { @@ -272,7 +372,7 @@ void b2ContactSolver::SolveVelocityConstraints() // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 // // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) - // b = vn_0 - velocityBias + // b = vn0 - velocityBias // // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases @@ -284,18 +384,23 @@ void b2ContactSolver::SolveVelocityConstraints() // // Substitute: // - // x = x' - a + // x = a + d // - // Plug into above equation: + // a := old total impulse + // x := new total impulse + // d := incremental impulse // - // vn = A * x + b - // = A * (x' - a) + b - // = A * x' + b - A * a - // = A * x' + b' + // For the current iteration we extend the formula for the incremental impulse + // to compute the new total impulse: + // + // vn = A * d + b + // = A * (x - a) + b + // = A * x + b - A * a + // = A * x + b' // b' = b - A * a; - b2ContactConstraintPoint* cp1 = c->points + 0; - b2ContactConstraintPoint* cp2 = c->points + 1; + b2VelocityConstraintPoint* cp1 = vc->points + 0; + b2VelocityConstraintPoint* cp2 = vc->points + 1; b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse); b2Assert(a.x >= 0.0f && a.y >= 0.0f); @@ -311,7 +416,9 @@ void b2ContactSolver::SolveVelocityConstraints() b2Vec2 b; b.x = vn1 - cp1->velocityBias; b.y = vn2 - cp2->velocityBias; - b -= b2Mul(c->K, a); + + // Compute b' + b -= b2Mul(vc->K, a); const float32 k_errorTol = 1e-3f; B2_NOT_USED(k_errorTol); @@ -321,27 +428,27 @@ void b2ContactSolver::SolveVelocityConstraints() // // Case 1: vn = 0 // - // 0 = A * x' + b' + // 0 = A * x + b' // - // Solve for x': + // Solve for x: // - // x' = - inv(A) * b' + // x = - inv(A) * b' // - b2Vec2 x = - b2Mul(c->normalMass, b); + b2Vec2 x = - b2Mul(vc->normalMass, b); if (x.x >= 0.0f && x.y >= 0.0f) { - // Resubstitute for the incremental impulse + // Get the incremental impulse b2Vec2 d = x - a; // Apply incremental impulse b2Vec2 P1 = d.x * normal; b2Vec2 P2 = d.y * normal; - vA -= invMassA * (P1 + P2); - wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); + vA -= mA * (P1 + P2); + wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - vB += invMassB * (P1 + P2); - wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); + vB += mB * (P1 + P2); + wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); // Accumulate cp1->normalImpulse = x.x; @@ -365,27 +472,27 @@ void b2ContactSolver::SolveVelocityConstraints() // // Case 2: vn1 = 0 and x2 = 0 // - // 0 = a11 * x1' + a12 * 0 + b1' - // vn2 = a21 * x1' + a22 * 0 + b2' + // 0 = a11 * x1 + a12 * 0 + b1' + // vn2 = a21 * x1 + a22 * 0 + b2' // x.x = - cp1->normalMass * b.x; x.y = 0.0f; vn1 = 0.0f; - vn2 = c->K.col1.y * x.x + b.y; + vn2 = vc->K.ex.y * x.x + b.y; if (x.x >= 0.0f && vn2 >= 0.0f) { - // Resubstitute for the incremental impulse + // Get the incremental impulse b2Vec2 d = x - a; // Apply incremental impulse b2Vec2 P1 = d.x * normal; b2Vec2 P2 = d.y * normal; - vA -= invMassA * (P1 + P2); - wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); + vA -= mA * (P1 + P2); + wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - vB += invMassB * (P1 + P2); - wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); + vB += mB * (P1 + P2); + wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); // Accumulate cp1->normalImpulse = x.x; @@ -407,12 +514,12 @@ void b2ContactSolver::SolveVelocityConstraints() // // Case 3: vn2 = 0 and x1 = 0 // - // vn1 = a11 * 0 + a12 * x2' + b1' - // 0 = a21 * 0 + a22 * x2' + b2' + // vn1 = a11 * 0 + a12 * x2 + b1' + // 0 = a21 * 0 + a22 * x2 + b2' // x.x = 0.0f; x.y = - cp2->normalMass * b.y; - vn1 = c->K.col2.x * x.y + b.x; + vn1 = vc->K.ey.x * x.y + b.x; vn2 = 0.0f; if (x.y >= 0.0f && vn1 >= 0.0f) @@ -423,11 +530,11 @@ void b2ContactSolver::SolveVelocityConstraints() // Apply incremental impulse b2Vec2 P1 = d.x * normal; b2Vec2 P2 = d.y * normal; - vA -= invMassA * (P1 + P2); - wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); + vA -= mA * (P1 + P2); + wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - vB += invMassB * (P1 + P2); - wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); + vB += mB * (P1 + P2); + wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); // Accumulate cp1->normalImpulse = x.x; @@ -463,11 +570,11 @@ void b2ContactSolver::SolveVelocityConstraints() // Apply incremental impulse b2Vec2 P1 = d.x * normal; b2Vec2 P2 = d.y * normal; - vA -= invMassA * (P1 + P2); - wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); + vA -= mA * (P1 + P2); + wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - vB += invMassB * (P1 + P2); - wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); + vB += mB * (P1 + P2); + wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); // Accumulate cp1->normalImpulse = x.x; @@ -481,73 +588,65 @@ void b2ContactSolver::SolveVelocityConstraints() } } - bodyA->m_linearVelocity = vA; - bodyA->m_angularVelocity = wA; - bodyB->m_linearVelocity = vB; - bodyB->m_angularVelocity = wB; + m_velocities[indexA].v = vA; + m_velocities[indexA].w = wA; + m_velocities[indexB].v = vB; + m_velocities[indexB].w = wB; } } void b2ContactSolver::StoreImpulses() { - for (int32 i = 0; i < m_constraintCount; ++i) + for (int32 i = 0; i < m_count; ++i) { - b2ContactConstraint* c = m_constraints + i; - b2Manifold* m = c->manifold; + b2ContactVelocityConstraint* vc = m_velocityConstraints + i; + b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold(); - for (int32 j = 0; j < c->pointCount; ++j) + for (int32 j = 0; j < vc->pointCount; ++j) { - m->points[j].normalImpulse = c->points[j].normalImpulse; - m->points[j].tangentImpulse = c->points[j].tangentImpulse; + manifold->points[j].normalImpulse = vc->points[j].normalImpulse; + manifold->points[j].tangentImpulse = vc->points[j].tangentImpulse; } } } struct b2PositionSolverManifold { - void Initialize(b2ContactConstraint* cc, int32 index) + void Initialize(b2ContactPositionConstraint* pc, const b2Transform& xfA, const b2Transform& xfB, int32 index) { - b2Assert(cc->pointCount > 0); + b2Assert(pc->pointCount > 0); - switch (cc->type) + switch (pc->type) { case b2Manifold::e_circles: { - b2Vec2 pointA = cc->bodyA->GetWorldPoint(cc->localPoint); - b2Vec2 pointB = cc->bodyB->GetWorldPoint(cc->points[0].localPoint); - if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon) - { - normal = pointB - pointA; - normal.Normalize(); - } - else - { - normal.Set(1.0f, 0.0f); - } - + b2Vec2 pointA = b2Mul(xfA, pc->localPoint); + b2Vec2 pointB = b2Mul(xfB, pc->localPoints[0]); + normal = pointB - pointA; + normal.Normalize(); point = 0.5f * (pointA + pointB); - separation = b2Dot(pointB - pointA, normal) - cc->radius; + separation = b2Dot(pointB - pointA, normal) - pc->radiusA - pc->radiusB; } break; case b2Manifold::e_faceA: { - normal = cc->bodyA->GetWorldVector(cc->localNormal); - b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint); + normal = b2Mul(xfA.q, pc->localNormal); + b2Vec2 planePoint = b2Mul(xfA, pc->localPoint); - b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->points[index].localPoint); - separation = b2Dot(clipPoint - planePoint, normal) - cc->radius; + b2Vec2 clipPoint = b2Mul(xfB, pc->localPoints[index]); + separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB; point = clipPoint; } break; case b2Manifold::e_faceB: { - normal = cc->bodyB->GetWorldVector(cc->localNormal); - b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint); + normal = b2Mul(xfB.q, pc->localNormal); + b2Vec2 planePoint = b2Mul(xfB, pc->localPoint); - b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->points[index].localPoint); - separation = b2Dot(clipPoint - planePoint, normal) - cc->radius; + b2Vec2 clipPoint = b2Mul(xfA, pc->localPoints[index]); + separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB; point = clipPoint; // Ensure normal points from A to B @@ -563,58 +662,168 @@ struct b2PositionSolverManifold }; // Sequential solver. -bool b2ContactSolver::SolvePositionConstraints(float32 baumgarte) +bool b2ContactSolver::SolvePositionConstraints() { float32 minSeparation = 0.0f; - for (int32 i = 0; i < m_constraintCount; ++i) + for (int32 i = 0; i < m_count; ++i) { - b2ContactConstraint* c = m_constraints + i; - b2Body* bodyA = c->bodyA; - b2Body* bodyB = c->bodyB; + b2ContactPositionConstraint* pc = m_positionConstraints + i; - float32 invMassA = bodyA->m_mass * bodyA->m_invMass; - float32 invIA = bodyA->m_mass * bodyA->m_invI; - float32 invMassB = bodyB->m_mass * bodyB->m_invMass; - float32 invIB = bodyB->m_mass * bodyB->m_invI; + int32 indexA = pc->indexA; + int32 indexB = pc->indexB; + b2Vec2 localCenterA = pc->localCenterA; + float32 mA = pc->invMassA; + float32 iA = pc->invIA; + b2Vec2 localCenterB = pc->localCenterB; + float32 mB = pc->invMassB; + float32 iB = pc->invIB; + int32 pointCount = pc->pointCount; + + b2Vec2 cA = m_positions[indexA].c; + float32 aA = m_positions[indexA].a; + + b2Vec2 cB = m_positions[indexB].c; + float32 aB = m_positions[indexB].a; // Solve normal constraints - for (int32 j = 0; j < c->pointCount; ++j) + for (int32 j = 0; j < pointCount; ++j) { + b2Transform xfA, xfB; + xfA.q.Set(aA); + xfB.q.Set(aB); + xfA.p = cA - b2Mul(xfA.q, localCenterA); + xfB.p = cB - b2Mul(xfB.q, localCenterB); + b2PositionSolverManifold psm; - psm.Initialize(c, j); + psm.Initialize(pc, xfA, xfB, j); b2Vec2 normal = psm.normal; b2Vec2 point = psm.point; float32 separation = psm.separation; - b2Vec2 rA = point - bodyA->m_sweep.c; - b2Vec2 rB = point - bodyB->m_sweep.c; + b2Vec2 rA = point - cA; + b2Vec2 rB = point - cB; // Track max constraint error. minSeparation = b2Min(minSeparation, separation); // Prevent large corrections and allow slop. - float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); + float32 C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); // Compute the effective mass. float32 rnA = b2Cross(rA, normal); float32 rnB = b2Cross(rB, normal); - float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; + float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse float32 impulse = K > 0.0f ? - C / K : 0.0f; b2Vec2 P = impulse * normal; - bodyA->m_sweep.c -= invMassA * P; - bodyA->m_sweep.a -= invIA * b2Cross(rA, P); - bodyA->SynchronizeTransform(); + cA -= mA * P; + aA -= iA * b2Cross(rA, P); - bodyB->m_sweep.c += invMassB * P; - bodyB->m_sweep.a += invIB * b2Cross(rB, P); - bodyB->SynchronizeTransform(); + cB += mB * P; + aB += iB * b2Cross(rB, P); } + + m_positions[indexA].c = cA; + m_positions[indexA].a = aA; + + m_positions[indexB].c = cB; + m_positions[indexB].a = aB; + } + + // We can't expect minSpeparation >= -b2_linearSlop because we don't + // push the separation above -b2_linearSlop. + return minSeparation >= -3.0f * b2_linearSlop; +} + +// Sequential position solver for position constraints. +bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB) +{ + float32 minSeparation = 0.0f; + + for (int32 i = 0; i < m_count; ++i) + { + b2ContactPositionConstraint* pc = m_positionConstraints + i; + + int32 indexA = pc->indexA; + int32 indexB = pc->indexB; + b2Vec2 localCenterA = pc->localCenterA; + b2Vec2 localCenterB = pc->localCenterB; + int32 pointCount = pc->pointCount; + + float32 mA = 0.0f; + float32 iA = 0.0f; + if (indexA == toiIndexA || indexA == toiIndexB) + { + mA = pc->invMassA; + iA = pc->invIA; + } + + float32 mB = pc->invMassB; + float32 iB = pc->invIB; + if (indexB == toiIndexA || indexB == toiIndexB) + { + mB = pc->invMassB; + iB = pc->invIB; + } + + b2Vec2 cA = m_positions[indexA].c; + float32 aA = m_positions[indexA].a; + + b2Vec2 cB = m_positions[indexB].c; + float32 aB = m_positions[indexB].a; + + // Solve normal constraints + for (int32 j = 0; j < pointCount; ++j) + { + b2Transform xfA, xfB; + xfA.q.Set(aA); + xfB.q.Set(aB); + xfA.p = cA - b2Mul(xfA.q, localCenterA); + xfB.p = cB - b2Mul(xfB.q, localCenterB); + + b2PositionSolverManifold psm; + psm.Initialize(pc, xfA, xfB, j); + b2Vec2 normal = psm.normal; + + b2Vec2 point = psm.point; + float32 separation = psm.separation; + + b2Vec2 rA = point - cA; + b2Vec2 rB = point - cB; + + // Track max constraint error. + minSeparation = b2Min(minSeparation, separation); + + // Prevent large corrections and allow slop. + float32 C = b2Clamp(b2_toiBaugarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); + + // Compute the effective mass. + float32 rnA = b2Cross(rA, normal); + float32 rnB = b2Cross(rB, normal); + float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + + // Compute normal impulse + float32 impulse = K > 0.0f ? - C / K : 0.0f; + + b2Vec2 P = impulse * normal; + + cA -= mA * P; + aA -= iA * b2Cross(rA, P); + + cB += mB * P; + aB += iB * b2Cross(rB, P); + } + + m_positions[indexA].c = cA; + m_positions[indexA].a = aA; + + m_positions[indexB].c = cB; + m_positions[indexB].a = aB; } // We can't expect minSpeparation >= -b2_linearSlop because we don't diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.h b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.h old mode 100644 new mode 100755 index b8555bbb4..ca9de04db --- a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -21,15 +21,15 @@ #include #include -#include +#include class b2Contact; class b2Body; class b2StackAllocator; +struct b2ContactPositionConstraint; -struct b2ContactConstraintPoint +struct b2VelocityConstraintPoint { - b2Vec2 localPoint; b2Vec2 rA; b2Vec2 rB; float32 normalImpulse; @@ -39,40 +39,56 @@ struct b2ContactConstraintPoint float32 velocityBias; }; -struct b2ContactConstraint +struct b2ContactVelocityConstraint { - b2ContactConstraintPoint points[b2_maxManifoldPoints]; - b2Vec2 localNormal; - b2Vec2 localPoint; + b2VelocityConstraintPoint points[b2_maxManifoldPoints]; b2Vec2 normal; b2Mat22 normalMass; b2Mat22 K; - b2Body* bodyA; - b2Body* bodyB; - b2Manifold::Type type; - float32 radius; + int32 indexA; + int32 indexB; + float32 invMassA, invMassB; + float32 invIA, invIB; float32 friction; + float32 restitution; int32 pointCount; - b2Manifold* manifold; + int32 contactIndex; +}; + +struct b2ContactSolverDef +{ + b2TimeStep step; + b2Contact** contacts; + int32 count; + b2Position* positions; + b2Velocity* velocities; + b2StackAllocator* allocator; }; class b2ContactSolver { public: - b2ContactSolver(b2Contact** contacts, int32 contactCount, - b2StackAllocator* allocator, float32 impulseRatio); - + b2ContactSolver(b2ContactSolverDef* def); ~b2ContactSolver(); + void InitializeVelocityConstraints(); + void WarmStart(); void SolveVelocityConstraints(); void StoreImpulses(); - bool SolvePositionConstraints(float32 baumgarte); + bool SolvePositionConstraints(); + bool SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB); + b2TimeStep m_step; + b2Position* m_positions; + b2Velocity* m_velocities; b2StackAllocator* m_allocator; - b2ContactConstraint* m_constraints; - int m_constraintCount; + b2ContactPositionConstraint* m_positionConstraints; + b2ContactVelocityConstraint* m_velocityConstraints; + b2Contact** m_contacts; + int m_count; }; #endif + diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp new file mode 100755 index 000000000..043365070 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp @@ -0,0 +1,50 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include + +#include +using namespace std; + +b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2EdgeAndCircleContact)); + return new (mem) b2EdgeAndCircleContact(fixtureA, fixtureB); +} + +void b2EdgeAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2EdgeAndCircleContact*)contact)->~b2EdgeAndCircleContact(); + allocator->Free(contact, sizeof(b2EdgeAndCircleContact)); +} + +b2EdgeAndCircleContact::b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) +: b2Contact(fixtureA, 0, fixtureB, 0) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_edge); + b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); +} + +void b2EdgeAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2CollideEdgeAndCircle( manifold, + (b2EdgeShape*)m_fixtureA->GetShape(), xfA, + (b2CircleShape*)m_fixtureB->GetShape(), xfB); +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h new file mode 100755 index 000000000..11b61b880 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h @@ -0,0 +1,39 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B2_EDGE_AND_CIRCLE_CONTACT_H +#define B2_EDGE_AND_CIRCLE_CONTACT_H + +#include + +class b2BlockAllocator; + +class b2EdgeAndCircleContact : public b2Contact +{ +public: + static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, + b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); + static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); + + b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); + ~b2EdgeAndCircleContact() {} + + void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); +}; + +#endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp new file mode 100755 index 000000000..8bac536a0 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp @@ -0,0 +1,50 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include + +#include +using namespace std; + +b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2EdgeAndPolygonContact)); + return new (mem) b2EdgeAndPolygonContact(fixtureA, fixtureB); +} + +void b2EdgeAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2EdgeAndPolygonContact*)contact)->~b2EdgeAndPolygonContact(); + allocator->Free(contact, sizeof(b2EdgeAndPolygonContact)); +} + +b2EdgeAndPolygonContact::b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB) +: b2Contact(fixtureA, 0, fixtureB, 0) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_edge); + b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); +} + +void b2EdgeAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2CollideEdgeAndPolygon( manifold, + (b2EdgeShape*)m_fixtureA->GetShape(), xfA, + (b2PolygonShape*)m_fixtureB->GetShape(), xfB); +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h new file mode 100755 index 000000000..74b27ee01 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h @@ -0,0 +1,39 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B2_EDGE_AND_POLYGON_CONTACT_H +#define B2_EDGE_AND_POLYGON_CONTACT_H + +#include + +class b2BlockAllocator; + +class b2EdgeAndPolygonContact : public b2Contact +{ +public: + static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, + b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); + static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); + + b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB); + ~b2EdgeAndPolygonContact() {} + + void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); +}; + +#endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp old mode 100644 new mode 100755 index 837722d92..880f83e22 --- a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -18,14 +18,12 @@ #include #include -#include -#include #include -#include #include +using namespace std; -b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator) +b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) { void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact)); return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB); @@ -38,7 +36,7 @@ void b2PolygonAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* al } b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) -: b2Contact(fixtureA, fixtureB) +: b2Contact(fixtureA, 0, fixtureB, 0) { b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon); b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h old mode 100644 new mode 100755 index 684b2ae04..6beca16a2 --- a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -26,7 +26,7 @@ class b2BlockAllocator; class b2PolygonAndCircleContact : public b2Contact { public: - static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator); + static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.cpp old mode 100644 new mode 100755 index eab2af579..52e1be874 --- a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -24,8 +24,9 @@ #include #include +using namespace std; -b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator) +b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) { void* mem = allocator->Allocate(sizeof(b2PolygonContact)); return new (mem) b2PolygonContact(fixtureA, fixtureB); @@ -38,7 +39,7 @@ void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) } b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB) - : b2Contact(fixtureA, fixtureB) + : b2Contact(fixtureA, 0, fixtureB, 0) { b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon); b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.h b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.h old mode 100644 new mode 100755 index af544c2e7..45932148c --- a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -26,7 +26,8 @@ class b2BlockAllocator; class b2PolygonContact : public b2Contact { public: - static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator); + static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, + b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB); diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2TOISolver.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2TOISolver.cpp deleted file mode 100644 index 567cd7125..000000000 --- a/src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2TOISolver.cpp +++ /dev/null @@ -1,231 +0,0 @@ -/* -* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include - -struct b2TOIConstraint -{ - b2Vec2 localPoints[b2_maxManifoldPoints]; - b2Vec2 localNormal; - b2Vec2 localPoint; - b2Manifold::Type type; - float32 radius; - int32 pointCount; - b2Body* bodyA; - b2Body* bodyB; -}; - -b2TOISolver::b2TOISolver(b2StackAllocator* allocator) -{ - m_allocator = allocator; - m_constraints = NULL; - m_count = NULL; - m_toiBody = NULL; -} - -b2TOISolver::~b2TOISolver() -{ - Clear(); -} - -void b2TOISolver::Clear() -{ - if (m_allocator && m_constraints) - { - m_allocator->Free(m_constraints); - m_constraints = NULL; - } -} - -void b2TOISolver::Initialize(b2Contact** contacts, int32 count, b2Body* toiBody) -{ - Clear(); - - m_count = count; - m_toiBody = toiBody; - - m_constraints = (b2TOIConstraint*) m_allocator->Allocate(m_count * sizeof(b2TOIConstraint)); - - for (int32 i = 0; i < m_count; ++i) - { - b2Contact* contact = contacts[i]; - - b2Fixture* fixtureA = contact->GetFixtureA(); - b2Fixture* fixtureB = contact->GetFixtureB(); - b2Shape* shapeA = fixtureA->GetShape(); - b2Shape* shapeB = fixtureB->GetShape(); - float32 radiusA = shapeA->m_radius; - float32 radiusB = shapeB->m_radius; - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - b2Manifold* manifold = contact->GetManifold(); - - b2Assert(manifold->pointCount > 0); - - b2TOIConstraint* constraint = m_constraints + i; - constraint->bodyA = bodyA; - constraint->bodyB = bodyB; - constraint->localNormal = manifold->localNormal; - constraint->localPoint = manifold->localPoint; - constraint->type = manifold->type; - constraint->pointCount = manifold->pointCount; - constraint->radius = radiusA + radiusB; - - for (int32 j = 0; j < constraint->pointCount; ++j) - { - b2ManifoldPoint* cp = manifold->points + j; - constraint->localPoints[j] = cp->localPoint; - } - } -} - -struct b2TOISolverManifold -{ - void Initialize(b2TOIConstraint* cc, int32 index) - { - b2Assert(cc->pointCount > 0); - - switch (cc->type) - { - case b2Manifold::e_circles: - { - b2Vec2 pointA = cc->bodyA->GetWorldPoint(cc->localPoint); - b2Vec2 pointB = cc->bodyB->GetWorldPoint(cc->localPoints[0]); - if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon) - { - normal = pointB - pointA; - normal.Normalize(); - } - else - { - normal.Set(1.0f, 0.0f); - } - - point = 0.5f * (pointA + pointB); - separation = b2Dot(pointB - pointA, normal) - cc->radius; - } - break; - - case b2Manifold::e_faceA: - { - normal = cc->bodyA->GetWorldVector(cc->localNormal); - b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint); - - b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->localPoints[index]); - separation = b2Dot(clipPoint - planePoint, normal) - cc->radius; - point = clipPoint; - } - break; - - case b2Manifold::e_faceB: - { - normal = cc->bodyB->GetWorldVector(cc->localNormal); - b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint); - - b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->localPoints[index]); - separation = b2Dot(clipPoint - planePoint, normal) - cc->radius; - point = clipPoint; - - // Ensure normal points from A to B - normal = -normal; - } - break; - } - } - - b2Vec2 normal; - b2Vec2 point; - float32 separation; -}; - -// Push out the toi body to provide clearance for further simulation. -bool b2TOISolver::Solve(float32 baumgarte) -{ - float32 minSeparation = 0.0f; - - for (int32 i = 0; i < m_count; ++i) - { - b2TOIConstraint* c = m_constraints + i; - b2Body* bodyA = c->bodyA; - b2Body* bodyB = c->bodyB; - - float32 massA = bodyA->m_mass; - float32 massB = bodyB->m_mass; - - // Only the TOI body should move. - if (bodyA == m_toiBody) - { - massB = 0.0f; - } - else - { - massA = 0.0f; - } - - float32 invMassA = massA * bodyA->m_invMass; - float32 invIA = massA * bodyA->m_invI; - float32 invMassB = massB * bodyB->m_invMass; - float32 invIB = massB * bodyB->m_invI; - - // Solve normal constraints - for (int32 j = 0; j < c->pointCount; ++j) - { - b2TOISolverManifold psm; - psm.Initialize(c, j); - b2Vec2 normal = psm.normal; - - b2Vec2 point = psm.point; - float32 separation = psm.separation; - - b2Vec2 rA = point - bodyA->m_sweep.c; - b2Vec2 rB = point - bodyB->m_sweep.c; - - // Track max constraint error. - minSeparation = b2Min(minSeparation, separation); - - // Prevent large corrections and allow slop. - float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); - - // Compute the effective mass. - float32 rnA = b2Cross(rA, normal); - float32 rnB = b2Cross(rB, normal); - float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; - - // Compute normal impulse - float32 impulse = K > 0.0f ? - C / K : 0.0f; - - b2Vec2 P = impulse * normal; - - bodyA->m_sweep.c -= invMassA * P; - bodyA->m_sweep.a -= invIA * b2Cross(rA, P); - bodyA->SynchronizeTransform(); - - bodyB->m_sweep.c += invMassB * P; - bodyB->m_sweep.a += invIB * b2Cross(rB, P); - bodyB->SynchronizeTransform(); - } - } - - // We can't expect minSpeparation >= -b2_linearSlop because we don't - // push the separation above -b2_linearSlop. - return minSeparation >= -1.5f * b2_linearSlop; -} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.cpp old mode 100644 new mode 100755 index 3469bd9e1..7b0e2a507 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -46,12 +46,11 @@ void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2, length = d.Length(); } - b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def) : b2Joint(def) { - m_localAnchor1 = def->localAnchorA; - m_localAnchor2 = def->localAnchorB; + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; m_length = def->length; m_frequencyHz = def->frequencyHz; m_dampingRatio = def->dampingRatio; @@ -60,15 +59,32 @@ b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def) m_bias = 0.0f; } -void b2DistanceJoint::InitVelocityConstraints(const b2TimeStep& step) +void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data) { - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; - // Compute the effective mass matrix. - b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); - m_u = b2->m_sweep.c + r2 - b1->m_sweep.c - r1; + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + m_u = cB + m_rB - cA - m_rA; // Handle singularity. float32 length = m_u.Length(); @@ -81,10 +97,11 @@ void b2DistanceJoint::InitVelocityConstraints(const b2TimeStep& step) m_u.Set(0.0f, 0.0f); } - float32 cr1u = b2Cross(r1, m_u); - float32 cr2u = b2Cross(r2, m_u); - float32 invMass = b1->m_invMass + b1->m_invI * cr1u * cr1u + b2->m_invMass + b2->m_invI * cr2u * cr2u; + float32 crAu = b2Cross(m_rA, m_u); + float32 crBu = b2Cross(m_rB, m_u); + float32 invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu; + // Compute the effective mass matrix. m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; if (m_frequencyHz > 0.0f) @@ -101,101 +118,111 @@ void b2DistanceJoint::InitVelocityConstraints(const b2TimeStep& step) float32 k = m_mass * omega * omega; // magic formulas - m_gamma = step.dt * (d + step.dt * k); + float32 h = data.step.dt; + m_gamma = h * (d + h * k); m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f; - m_bias = C * step.dt * k * m_gamma; + m_bias = C * h * k * m_gamma; m_mass = invMass + m_gamma; m_mass = m_mass != 0.0f ? 1.0f / m_mass : 0.0f; } - if (step.warmStarting) + if (data.step.warmStarting) { // Scale the impulse to support a variable time step. - m_impulse *= step.dtRatio; + m_impulse *= data.step.dtRatio; b2Vec2 P = m_impulse * m_u; - b1->m_linearVelocity -= b1->m_invMass * P; - b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P); - b2->m_linearVelocity += b2->m_invMass * P; - b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P); + vA -= m_invMassA * P; + wA -= m_invIA * b2Cross(m_rA, P); + vB += m_invMassB * P; + wB += m_invIB * b2Cross(m_rB, P); } else { m_impulse = 0.0f; } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2DistanceJoint::SolveVelocityConstraints(const b2TimeStep& step) +void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data) { - B2_NOT_USED(step); - - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; - - b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; // Cdot = dot(u, v + cross(w, r)) - b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1); - b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2); - float32 Cdot = b2Dot(m_u, v2 - v1); + b2Vec2 vpA = vA + b2Cross(wA, m_rA); + b2Vec2 vpB = vB + b2Cross(wB, m_rB); + float32 Cdot = b2Dot(m_u, vpB - vpA); float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse); m_impulse += impulse; b2Vec2 P = impulse * m_u; - b1->m_linearVelocity -= b1->m_invMass * P; - b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P); - b2->m_linearVelocity += b2->m_invMass * P; - b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P); + vA -= m_invMassA * P; + wA -= m_invIA * b2Cross(m_rA, P); + vB += m_invMassB * P; + wB += m_invIB * b2Cross(m_rB, P); + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -bool b2DistanceJoint::SolvePositionConstraints(float32 baumgarte) +bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data) { - B2_NOT_USED(baumgarte); - if (m_frequencyHz > 0.0f) { // There is no position correction for soft distance constraints. return true; } - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; - b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); + b2Rot qA(aA), qB(aB); - b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1; + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + b2Vec2 u = cB + rB - cA - rA; - float32 length = d.Normalize(); + float32 length = u.Normalize(); float32 C = length - m_length; C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection); float32 impulse = -m_mass * C; - m_u = d; - b2Vec2 P = impulse * m_u; + b2Vec2 P = impulse * u; - b1->m_sweep.c -= b1->m_invMass * P; - b1->m_sweep.a -= b1->m_invI * b2Cross(r1, P); - b2->m_sweep.c += b2->m_invMass * P; - b2->m_sweep.a += b2->m_invI * b2Cross(r2, P); + cA -= m_invMassA * P; + aA -= m_invIA * b2Cross(rA, P); + cB += m_invMassB * P; + aB += m_invIB * b2Cross(rB, P); - b1->SynchronizeTransform(); - b2->SynchronizeTransform(); + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; return b2Abs(C) < b2_linearSlop; } b2Vec2 b2DistanceJoint::GetAnchorA() const { - return m_bodyA->GetWorldPoint(m_localAnchor1); + return m_bodyA->GetWorldPoint(m_localAnchorA); } b2Vec2 b2DistanceJoint::GetAnchorB() const { - return m_bodyB->GetWorldPoint(m_localAnchor2); + return m_bodyB->GetWorldPoint(m_localAnchorB); } b2Vec2 b2DistanceJoint::GetReactionForce(float32 inv_dt) const diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.h old mode 100644 new mode 100755 index 448faa668..a8d35a5e6 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -70,7 +70,12 @@ public: b2Vec2 GetAnchorA() const; b2Vec2 GetAnchorB() const; + /// Get the reaction force given the inverse time step. + /// Unit is N. b2Vec2 GetReactionForce(float32 inv_dt) const; + + /// Get the reaction torque given the inverse time step. + /// Unit is N*m. This is always zero for a distance joint. float32 GetReactionTorque(float32 inv_dt) const; /// Set/get the natural length. @@ -91,20 +96,34 @@ protected: friend class b2Joint; b2DistanceJoint(const b2DistanceJointDef* data); - void InitVelocityConstraints(const b2TimeStep& step); - void SolveVelocityConstraints(const b2TimeStep& step); - bool SolvePositionConstraints(float32 baumgarte); + void InitVelocityConstraints(const b2SolverData& data); + void SolveVelocityConstraints(const b2SolverData& data); + bool SolvePositionConstraints(const b2SolverData& data); - b2Vec2 m_localAnchor1; - b2Vec2 m_localAnchor2; - b2Vec2 m_u; float32 m_frequencyHz; float32 m_dampingRatio; - float32 m_gamma; float32 m_bias; + + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + float32 m_gamma; float32 m_impulse; - float32 m_mass; float32 m_length; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_u; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + float32 m_mass; }; inline void b2DistanceJoint::SetLength(float32 length) diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.cpp old mode 100644 new mode 100755 index 9097deec3..8c363e6a1 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -53,14 +53,30 @@ b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def) m_maxTorque = def->maxTorque; } -void b2FrictionJoint::InitVelocityConstraints(const b2TimeStep& step) +void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data) { - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; + + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); // Compute the effective mass matrix. - b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter()); - b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter()); + m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] @@ -71,22 +87,15 @@ void b2FrictionJoint::InitVelocityConstraints(const b2TimeStep& step) // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - float32 mA = bA->m_invMass, mB = bB->m_invMass; - float32 iA = bA->m_invI, iB = bB->m_invI; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; - b2Mat22 K1; - K1.col1.x = mA + mB; K1.col2.x = 0.0f; - K1.col1.y = 0.0f; K1.col2.y = mA + mB; + b2Mat22 K; + K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y; + K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y; + K.ey.x = K.ex.y; + K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x; - b2Mat22 K2; - K2.col1.x = iA * rA.y * rA.y; K2.col2.x = -iA * rA.x * rA.y; - K2.col1.y = -iA * rA.x * rA.y; K2.col2.y = iA * rA.x * rA.x; - - b2Mat22 K3; - K3.col1.x = iB * rB.y * rB.y; K3.col2.x = -iB * rB.x * rB.y; - K3.col1.y = -iB * rB.x * rB.y; K3.col2.y = iB * rB.x * rB.x; - - b2Mat22 K = K1 + K2 + K3; m_linearMass = K.GetInverse(); m_angularMass = iA + iB; @@ -95,44 +104,41 @@ void b2FrictionJoint::InitVelocityConstraints(const b2TimeStep& step) m_angularMass = 1.0f / m_angularMass; } - if (step.warmStarting) + if (data.step.warmStarting) { // Scale impulses to support a variable time step. - m_linearImpulse *= step.dtRatio; - m_angularImpulse *= step.dtRatio; + m_linearImpulse *= data.step.dtRatio; + m_angularImpulse *= data.step.dtRatio; b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y); - - bA->m_linearVelocity -= mA * P; - bA->m_angularVelocity -= iA * (b2Cross(rA, P) + m_angularImpulse); - - bB->m_linearVelocity += mB * P; - bB->m_angularVelocity += iB * (b2Cross(rB, P) + m_angularImpulse); + vA -= mA * P; + wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse); + vB += mB * P; + wB += iB * (b2Cross(m_rB, P) + m_angularImpulse); } else { m_linearImpulse.SetZero(); m_angularImpulse = 0.0f; } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2FrictionJoint::SolveVelocityConstraints(const b2TimeStep& step) +void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data) { - B2_NOT_USED(step); + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; - b2Vec2 vA = bA->m_linearVelocity; - float32 wA = bA->m_angularVelocity; - b2Vec2 vB = bB->m_linearVelocity; - float32 wB = bB->m_angularVelocity; - - float32 mA = bA->m_invMass, mB = bB->m_invMass; - float32 iA = bA->m_invI, iB = bB->m_invI; - - b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter()); - b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter()); + float32 h = data.step.dt; // Solve angular friction { @@ -140,7 +146,7 @@ void b2FrictionJoint::SolveVelocityConstraints(const b2TimeStep& step) float32 impulse = -m_angularMass * Cdot; float32 oldImpulse = m_angularImpulse; - float32 maxImpulse = step.dt * m_maxTorque; + float32 maxImpulse = h * m_maxTorque; m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_angularImpulse - oldImpulse; @@ -150,13 +156,13 @@ void b2FrictionJoint::SolveVelocityConstraints(const b2TimeStep& step) // Solve linear friction { - b2Vec2 Cdot = vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA); + b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); b2Vec2 impulse = -b2Mul(m_linearMass, Cdot); b2Vec2 oldImpulse = m_linearImpulse; m_linearImpulse += impulse; - float32 maxImpulse = step.dt * m_maxForce; + float32 maxImpulse = h * m_maxForce; if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) { @@ -167,21 +173,21 @@ void b2FrictionJoint::SolveVelocityConstraints(const b2TimeStep& step) impulse = m_linearImpulse - oldImpulse; vA -= mA * impulse; - wA -= iA * b2Cross(rA, impulse); + wA -= iA * b2Cross(m_rA, impulse); vB += mB * impulse; - wB += iB * b2Cross(rB, impulse); + wB += iB * b2Cross(m_rB, impulse); } - bA->m_linearVelocity = vA; - bA->m_angularVelocity = wA; - bB->m_linearVelocity = vB; - bB->m_angularVelocity = wB; + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -bool b2FrictionJoint::SolvePositionConstraints(float32 baumgarte) +bool b2FrictionJoint::SolvePositionConstraints(const b2SolverData& data) { - B2_NOT_USED(baumgarte); + B2_NOT_USED(data); return true; } diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.h old mode 100644 new mode 100755 index b4c4af261..3e72be2fe --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -79,21 +79,32 @@ protected: b2FrictionJoint(const b2FrictionJointDef* def); - void InitVelocityConstraints(const b2TimeStep& step); - void SolveVelocityConstraints(const b2TimeStep& step); - bool SolvePositionConstraints(float32 baumgarte); + void InitVelocityConstraints(const b2SolverData& data); + void SolveVelocityConstraints(const b2SolverData& data); + bool SolvePositionConstraints(const b2SolverData& data); b2Vec2 m_localAnchorA; b2Vec2 m_localAnchorB; - b2Mat22 m_linearMass; - float32 m_angularMass; - + // Solver shared b2Vec2 m_linearImpulse; float32 m_angularImpulse; - float32 m_maxForce; float32 m_maxTorque; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + b2Mat22 m_linearMass; + float32 m_angularMass; }; #endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.cpp old mode 100644 new mode 100755 index 89b17ee2c..cff95f2bc --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2007-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -24,9 +24,8 @@ // Gear Joint: // C0 = (coordinate1 + ratio * coordinate2)_initial -// C = C0 - (cordinate1 + ratio * coordinate2) = 0 -// Cdot = -(Cdot1 + ratio * Cdot2) -// J = -[J1 ratio * J2] +// C = (coordinate1 + ratio * coordinate2) - C0 = 0 +// J = [J1 ratio * J2] // K = J * invM * JT // = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T // @@ -45,177 +44,323 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def) : b2Joint(def) { - b2JointType type1 = def->joint1->GetType(); - b2JointType type2 = def->joint2->GetType(); + m_typeA = def->joint1->GetType(); + m_typeB = def->joint2->GetType(); - b2Assert(type1 == e_revoluteJoint || type1 == e_prismaticJoint); - b2Assert(type2 == e_revoluteJoint || type2 == e_prismaticJoint); - b2Assert(def->joint1->GetBodyA()->GetType() == b2_staticBody); - b2Assert(def->joint2->GetBodyA()->GetType() == b2_staticBody); + b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint); + b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint); - m_revolute1 = NULL; - m_prismatic1 = NULL; - m_revolute2 = NULL; - m_prismatic2 = NULL; + float32 coordinateA, coordinateB; - float32 coordinate1, coordinate2; - - m_ground1 = def->joint1->GetBodyA(); + m_bodyC = def->joint1->GetBodyA(); m_bodyA = def->joint1->GetBodyB(); - if (type1 == e_revoluteJoint) + + // Get geometry of joint1 + b2Transform xfA = m_bodyA->m_xf; + float32 aA = m_bodyA->m_sweep.a; + b2Transform xfC = m_bodyC->m_xf; + float32 aC = m_bodyC->m_sweep.a; + + if (m_typeA == e_revoluteJoint) { - m_revolute1 = (b2RevoluteJoint*)def->joint1; - m_groundAnchor1 = m_revolute1->m_localAnchor1; - m_localAnchor1 = m_revolute1->m_localAnchor2; - coordinate1 = m_revolute1->GetJointAngle(); + b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1; + m_localAnchorC = revolute->m_localAnchorA; + m_localAnchorA = revolute->m_localAnchorB; + m_referenceAngleA = revolute->m_referenceAngle; + m_localAxisC.SetZero(); + + coordinateA = aA - aC - m_referenceAngleA; } else { - m_prismatic1 = (b2PrismaticJoint*)def->joint1; - m_groundAnchor1 = m_prismatic1->m_localAnchor1; - m_localAnchor1 = m_prismatic1->m_localAnchor2; - coordinate1 = m_prismatic1->GetJointTranslation(); + b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1; + m_localAnchorC = prismatic->m_localAnchorA; + m_localAnchorA = prismatic->m_localAnchorB; + m_referenceAngleA = prismatic->m_refAngle; + m_localAxisC = prismatic->m_localXAxisA; + + b2Vec2 pC = m_localAnchorC; + b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p)); + coordinateA = b2Dot(pA - pC, m_localAxisC); } - m_ground2 = def->joint2->GetBodyA(); + m_bodyD = def->joint2->GetBodyA(); m_bodyB = def->joint2->GetBodyB(); - if (type2 == e_revoluteJoint) + + // Get geometry of joint2 + b2Transform xfB = m_bodyB->m_xf; + float32 aB = m_bodyB->m_sweep.a; + b2Transform xfD = m_bodyD->m_xf; + float32 aD = m_bodyD->m_sweep.a; + + if (m_typeB == e_revoluteJoint) { - m_revolute2 = (b2RevoluteJoint*)def->joint2; - m_groundAnchor2 = m_revolute2->m_localAnchor1; - m_localAnchor2 = m_revolute2->m_localAnchor2; - coordinate2 = m_revolute2->GetJointAngle(); + b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2; + m_localAnchorD = revolute->m_localAnchorA; + m_localAnchorB = revolute->m_localAnchorB; + m_referenceAngleB = revolute->m_referenceAngle; + m_localAxisD.SetZero(); + + coordinateB = aB - aD - m_referenceAngleB; } else { - m_prismatic2 = (b2PrismaticJoint*)def->joint2; - m_groundAnchor2 = m_prismatic2->m_localAnchor1; - m_localAnchor2 = m_prismatic2->m_localAnchor2; - coordinate2 = m_prismatic2->GetJointTranslation(); + b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2; + m_localAnchorD = prismatic->m_localAnchorA; + m_localAnchorB = prismatic->m_localAnchorB; + m_referenceAngleB = prismatic->m_refAngle; + m_localAxisD = prismatic->m_localXAxisA; + + b2Vec2 pD = m_localAnchorD; + b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p)); + coordinateB = b2Dot(pB - pD, m_localAxisD); } m_ratio = def->ratio; - m_constant = coordinate1 + m_ratio * coordinate2; + m_constant = coordinateA + m_ratio * coordinateB; m_impulse = 0.0f; } -void b2GearJoint::InitVelocityConstraints(const b2TimeStep& step) +void b2GearJoint::InitVelocityConstraints(const b2SolverData& data) { - b2Body* g1 = m_ground1; - b2Body* g2 = m_ground2; - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_indexC = m_bodyC->m_islandIndex; + m_indexD = m_bodyD->m_islandIndex; + m_lcA = m_bodyA->m_sweep.localCenter; + m_lcB = m_bodyB->m_sweep.localCenter; + m_lcC = m_bodyC->m_sweep.localCenter; + m_lcD = m_bodyD->m_sweep.localCenter; + m_mA = m_bodyA->m_invMass; + m_mB = m_bodyB->m_invMass; + m_mC = m_bodyC->m_invMass; + m_mD = m_bodyD->m_invMass; + m_iA = m_bodyA->m_invI; + m_iB = m_bodyB->m_invI; + m_iC = m_bodyC->m_invI; + m_iD = m_bodyD->m_invI; - float32 K = 0.0f; - m_J.SetZero(); + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; - if (m_revolute1) + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Vec2 cC = data.positions[m_indexC].c; + float32 aC = data.positions[m_indexC].a; + b2Vec2 vC = data.velocities[m_indexC].v; + float32 wC = data.velocities[m_indexC].w; + + b2Vec2 cD = data.positions[m_indexD].c; + float32 aD = data.positions[m_indexD].a; + b2Vec2 vD = data.velocities[m_indexD].v; + float32 wD = data.velocities[m_indexD].w; + + b2Rot qA(aA), qB(aB), qC(aC), qD(aD); + + m_mass = 0.0f; + + if (m_typeA == e_revoluteJoint) { - m_J.angularA = -1.0f; - K += b1->m_invI; + m_JvAC.SetZero(); + m_JwA = 1.0f; + m_JwC = 1.0f; + m_mass += m_iA + m_iC; } else { - b2Vec2 ug = b2Mul(g1->GetTransform().R, m_prismatic1->m_localXAxis1); - b2Vec2 r = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - float32 crug = b2Cross(r, ug); - m_J.linearA = -ug; - m_J.angularA = -crug; - K += b1->m_invMass + b1->m_invI * crug * crug; + b2Vec2 u = b2Mul(qC, m_localAxisC); + b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); + m_JvAC = u; + m_JwC = b2Cross(rC, u); + m_JwA = b2Cross(rA, u); + m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA; } - if (m_revolute2) + if (m_typeB == e_revoluteJoint) { - m_J.angularB = -m_ratio; - K += m_ratio * m_ratio * b2->m_invI; + m_JvBD.SetZero(); + m_JwB = m_ratio; + m_JwD = m_ratio; + m_mass += m_ratio * m_ratio * (m_iB + m_iD); } else { - b2Vec2 ug = b2Mul(g2->GetTransform().R, m_prismatic2->m_localXAxis1); - b2Vec2 r = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); - float32 crug = b2Cross(r, ug); - m_J.linearB = -m_ratio * ug; - m_J.angularB = -m_ratio * crug; - K += m_ratio * m_ratio * (b2->m_invMass + b2->m_invI * crug * crug); + b2Vec2 u = b2Mul(qD, m_localAxisD); + b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); + m_JvBD = m_ratio * u; + m_JwD = m_ratio * b2Cross(rD, u); + m_JwB = m_ratio * b2Cross(rB, u); + m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB; } // Compute effective mass. - m_mass = K > 0.0f ? 1.0f / K : 0.0f; + m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f; - if (step.warmStarting) + if (data.step.warmStarting) { - // Warm starting. - b1->m_linearVelocity += b1->m_invMass * m_impulse * m_J.linearA; - b1->m_angularVelocity += b1->m_invI * m_impulse * m_J.angularA; - b2->m_linearVelocity += b2->m_invMass * m_impulse * m_J.linearB; - b2->m_angularVelocity += b2->m_invI * m_impulse * m_J.angularB; + vA += (m_mA * m_impulse) * m_JvAC; + wA += m_iA * m_impulse * m_JwA; + vB += (m_mB * m_impulse) * m_JvBD; + wB += m_iB * m_impulse * m_JwB; + vC -= (m_mC * m_impulse) * m_JvAC; + wC -= m_iC * m_impulse * m_JwC; + vD -= (m_mD * m_impulse) * m_JvBD; + wD -= m_iD * m_impulse * m_JwD; } else { m_impulse = 0.0f; } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; + data.velocities[m_indexC].v = vC; + data.velocities[m_indexC].w = wC; + data.velocities[m_indexD].v = vD; + data.velocities[m_indexD].w = wD; } -void b2GearJoint::SolveVelocityConstraints(const b2TimeStep& step) +void b2GearJoint::SolveVelocityConstraints(const b2SolverData& data) { - B2_NOT_USED(step); + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + b2Vec2 vC = data.velocities[m_indexC].v; + float32 wC = data.velocities[m_indexC].w; + b2Vec2 vD = data.velocities[m_indexD].v; + float32 wD = data.velocities[m_indexD].w; - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; + float32 Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD); + Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD); - float32 Cdot = m_J.Compute( b1->m_linearVelocity, b1->m_angularVelocity, - b2->m_linearVelocity, b2->m_angularVelocity); - - float32 impulse = m_mass * (-Cdot); + float32 impulse = -m_mass * Cdot; m_impulse += impulse; - b1->m_linearVelocity += b1->m_invMass * impulse * m_J.linearA; - b1->m_angularVelocity += b1->m_invI * impulse * m_J.angularA; - b2->m_linearVelocity += b2->m_invMass * impulse * m_J.linearB; - b2->m_angularVelocity += b2->m_invI * impulse * m_J.angularB; + vA += (m_mA * impulse) * m_JvAC; + wA += m_iA * impulse * m_JwA; + vB += (m_mB * impulse) * m_JvBD; + wB += m_iB * impulse * m_JwB; + vC -= (m_mC * impulse) * m_JvAC; + wC -= m_iC * impulse * m_JwC; + vD -= (m_mD * impulse) * m_JvBD; + wD -= m_iD * impulse * m_JwD; + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; + data.velocities[m_indexC].v = vC; + data.velocities[m_indexC].w = wC; + data.velocities[m_indexD].v = vD; + data.velocities[m_indexD].w = wD; } -bool b2GearJoint::SolvePositionConstraints(float32 baumgarte) +bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data) { - B2_NOT_USED(baumgarte); - + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 cC = data.positions[m_indexC].c; + float32 aC = data.positions[m_indexC].a; + b2Vec2 cD = data.positions[m_indexD].c; + float32 aD = data.positions[m_indexD].a; + + b2Rot qA(aA), qB(aB), qC(aC), qD(aD); + float32 linearError = 0.0f; - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; + float32 coordinateA, coordinateB; - float32 coordinate1, coordinate2; - if (m_revolute1) + b2Vec2 JvAC, JvBD; + float32 JwA, JwB, JwC, JwD; + float32 mass = 0.0f; + + if (m_typeA == e_revoluteJoint) { - coordinate1 = m_revolute1->GetJointAngle(); + JvAC.SetZero(); + JwA = 1.0f; + JwC = 1.0f; + mass += m_iA + m_iC; + + coordinateA = aA - aC - m_referenceAngleA; } else { - coordinate1 = m_prismatic1->GetJointTranslation(); + b2Vec2 u = b2Mul(qC, m_localAxisC); + b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); + JvAC = u; + JwC = b2Cross(rC, u); + JwA = b2Cross(rA, u); + mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA; + + b2Vec2 pC = m_localAnchorC - m_lcC; + b2Vec2 pA = b2MulT(qC, rA + (cA - cC)); + coordinateA = b2Dot(pA - pC, m_localAxisC); } - if (m_revolute2) + if (m_typeB == e_revoluteJoint) { - coordinate2 = m_revolute2->GetJointAngle(); + JvBD.SetZero(); + JwB = 1.0f; + JwD = 1.0f; + mass += m_iB + m_iD; + + coordinateB = aB - aD - m_referenceAngleB; } else { - coordinate2 = m_prismatic2->GetJointTranslation(); + b2Vec2 u = b2Mul(qD, m_localAxisD); + b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); + JvBD = m_ratio * u; + JwD = m_ratio * b2Cross(rD, u); + JwB = m_ratio * b2Cross(rB, u); + mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB; + + b2Vec2 pD = m_localAnchorD - m_lcD; + b2Vec2 pB = b2MulT(qD, rB + (cB - cD)); + coordinateB = b2Dot(pB - pD, m_localAxisD); } - float32 C = m_constant - (coordinate1 + m_ratio * coordinate2); + float32 C = (coordinateA + m_ratio * coordinateB) - m_constant; - float32 impulse = m_mass * (-C); + float32 impulse = 0.0f; + if (mass > 0.0f) + { + impulse = -C / mass; + } - b1->m_sweep.c += b1->m_invMass * impulse * m_J.linearA; - b1->m_sweep.a += b1->m_invI * impulse * m_J.angularA; - b2->m_sweep.c += b2->m_invMass * impulse * m_J.linearB; - b2->m_sweep.a += b2->m_invI * impulse * m_J.angularB; + cA += m_mA * impulse * JvAC; + aA += m_iA * impulse * JwA; + cB += m_mB * impulse * JvBD; + aB += m_iB * impulse * JwB; + cC -= m_mC * impulse * JvAC; + aC -= m_iC * impulse * JwC; + cD -= m_mD * impulse * JvBD; + aD -= m_iD * impulse * JwD; - b1->SynchronizeTransform(); - b2->SynchronizeTransform(); + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; + data.positions[m_indexC].c = cC; + data.positions[m_indexC].a = aC; + data.positions[m_indexD].c = cD; + data.positions[m_indexD].a = aD; // TODO_ERIN not implemented return linearError < b2_linearSlop; @@ -223,27 +368,23 @@ bool b2GearJoint::SolvePositionConstraints(float32 baumgarte) b2Vec2 b2GearJoint::GetAnchorA() const { - return m_bodyA->GetWorldPoint(m_localAnchor1); + return m_bodyA->GetWorldPoint(m_localAnchorA); } b2Vec2 b2GearJoint::GetAnchorB() const { - return m_bodyB->GetWorldPoint(m_localAnchor2); + return m_bodyB->GetWorldPoint(m_localAnchorB); } b2Vec2 b2GearJoint::GetReactionForce(float32 inv_dt) const { - // TODO_ERIN not tested - b2Vec2 P = m_impulse * m_J.linearB; + b2Vec2 P = m_impulse * m_JvAC; return inv_dt * P; } float32 b2GearJoint::GetReactionTorque(float32 inv_dt) const { - // TODO_ERIN not tested - b2Vec2 r = b2Mul(m_bodyB->GetTransform().R, m_localAnchor2 - m_bodyB->GetLocalCenter()); - b2Vec2 P = m_impulse * m_J.linearB; - float32 L = m_impulse * m_J.angularB - b2Cross(r, P); + float32 L = m_impulse * m_JwA; return inv_dt * L; } diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.h old mode 100644 new mode 100755 index eccca0da0..5651d5418 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -21,12 +21,8 @@ #include -class b2RevoluteJoint; -class b2PrismaticJoint; - /// Gear joint definition. This definition requires two existing /// revolute or prismatic joints (any combination will work). -/// The provided joints must attach a dynamic body to a static body. struct b2GearJointDef : public b2JointDef { b2GearJointDef() @@ -55,8 +51,8 @@ struct b2GearJointDef : public b2JointDef /// The ratio can be negative or positive. If one joint is a revolute joint /// and the other joint is a prismatic joint, then the ratio will have units /// of length or units of 1/length. -/// @warning The revolute and prismatic joints must be attached to -/// fixed bodies (which must be body1 on those joints). +/// @warning You have to manually destroy the gear joint if joint1 or joint2 +/// is destroyed. class b2GearJoint : public b2Joint { public: @@ -75,37 +71,43 @@ protected: friend class b2Joint; b2GearJoint(const b2GearJointDef* data); - void InitVelocityConstraints(const b2TimeStep& step); - void SolveVelocityConstraints(const b2TimeStep& step); - bool SolvePositionConstraints(float32 baumgarte); + void InitVelocityConstraints(const b2SolverData& data); + void SolveVelocityConstraints(const b2SolverData& data); + bool SolvePositionConstraints(const b2SolverData& data); - b2Body* m_ground1; - b2Body* m_ground2; + b2JointType m_typeA; + b2JointType m_typeB; - // One of these is NULL. - b2RevoluteJoint* m_revolute1; - b2PrismaticJoint* m_prismatic1; + // Body A is connected to body C + // Body B is connected to body D + b2Body* m_bodyC; + b2Body* m_bodyD; - // One of these is NULL. - b2RevoluteJoint* m_revolute2; - b2PrismaticJoint* m_prismatic2; + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + b2Vec2 m_localAnchorC; + b2Vec2 m_localAnchorD; - b2Vec2 m_groundAnchor1; - b2Vec2 m_groundAnchor2; + b2Vec2 m_localAxisC; + b2Vec2 m_localAxisD; - b2Vec2 m_localAnchor1; - b2Vec2 m_localAnchor2; - - b2Jacobian m_J; + float32 m_referenceAngleA; + float32 m_referenceAngleB; float32 m_constant; float32 m_ratio; - // Effective mass - float32 m_mass; - - // Impulse for accumulation/warm starting. float32 m_impulse; + + // Solver temp + int32 m_indexA, m_indexB, m_indexC, m_indexD; + b2Vec2 m_lcA, m_lcB, m_lcC, m_lcD; + float32 m_mA, m_mB, m_mC, m_mD; + float32 m_iA, m_iB, m_iC, m_iD; + b2Vec2 m_JvAC, m_JvBD; + float32 m_JwA, m_JwB, m_JwC, m_JwD; + float32 m_mass; }; #endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.cpp old mode 100644 new mode 100755 index a7e19d34d..eee195a9d --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -80,10 +81,10 @@ b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator) } break; - case e_lineJoint: + case e_wheelJoint: { - void* mem = allocator->Allocate(sizeof(b2LineJoint)); - joint = new (mem) b2LineJoint((b2LineJointDef*)def); + void* mem = allocator->Allocate(sizeof(b2WheelJoint)); + joint = new (mem) b2WheelJoint((b2WheelJointDef*)def); } break; @@ -101,6 +102,13 @@ b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator) } break; + case e_ropeJoint: + { + void* mem = allocator->Allocate(sizeof(b2RopeJoint)); + joint = new (mem) b2RopeJoint((b2RopeJointDef*)def); + } + break; + default: b2Assert(false); break; @@ -138,8 +146,8 @@ void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator) allocator->Free(joint, sizeof(b2GearJoint)); break; - case e_lineJoint: - allocator->Free(joint, sizeof(b2LineJoint)); + case e_wheelJoint: + allocator->Free(joint, sizeof(b2WheelJoint)); break; case e_weldJoint: @@ -150,6 +158,10 @@ void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator) allocator->Free(joint, sizeof(b2FrictionJoint)); break; + case e_ropeJoint: + allocator->Free(joint, sizeof(b2RopeJoint)); + break; + default: b2Assert(false); break; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.h old mode 100644 new mode 100755 index 213ad7d1d..ed6ad4103 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -23,7 +23,7 @@ class b2Body; class b2Joint; -struct b2TimeStep; +struct b2SolverData; class b2BlockAllocator; enum b2JointType @@ -35,9 +35,10 @@ enum b2JointType e_pulleyJoint, e_mouseJoint, e_gearJoint, - e_lineJoint, + e_wheelJoint, e_weldJoint, e_frictionJoint, + e_ropeJoint }; enum b2LimitState @@ -50,14 +51,9 @@ enum b2LimitState struct b2Jacobian { - b2Vec2 linearA; + b2Vec2 linear; float32 angularA; - b2Vec2 linearB; float32 angularB; - - void SetZero(); - void Set(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2); - float32 Compute(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2); }; /// A joint edge is used to connect bodies and joints together @@ -130,6 +126,7 @@ public: /// Get the next joint the world joint list. b2Joint* GetNext(); + const b2Joint* GetNext() const; /// Get the user data pointer. void* GetUserData() const; @@ -140,6 +137,11 @@ public: /// Short-cut function to determine if either body is inactive. bool IsActive() const; + /// Get collide connected. + /// Note: modifying the collide connect flag won't work correctly because + /// the flag is only checked when fixture AABBs begin to overlap. + bool GetCollideConnected() const; + protected: friend class b2World; friend class b2Body; @@ -151,11 +153,11 @@ protected: b2Joint(const b2JointDef* def); virtual ~b2Joint() {} - virtual void InitVelocityConstraints(const b2TimeStep& step) = 0; - virtual void SolveVelocityConstraints(const b2TimeStep& step) = 0; + virtual void InitVelocityConstraints(const b2SolverData& data) = 0; + virtual void SolveVelocityConstraints(const b2SolverData& data) = 0; // This returns true if the position errors are within tolerance. - virtual bool SolvePositionConstraints(float32 baumgarte) = 0; + virtual bool SolvePositionConstraints(const b2SolverData& data) = 0; b2JointType m_type; b2Joint* m_prev; @@ -169,30 +171,8 @@ protected: bool m_collideConnected; void* m_userData; - - // Cache here per time step to reduce cache misses. - b2Vec2 m_localCenterA, m_localCenterB; - float32 m_invMassA, m_invIA; - float32 m_invMassB, m_invIB; }; -inline void b2Jacobian::SetZero() -{ - linearA.SetZero(); angularA = 0.0f; - linearB.SetZero(); angularB = 0.0f; -} - -inline void b2Jacobian::Set(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2) -{ - linearA = x1; angularA = a1; - linearB = x2; angularB = a2; -} - -inline float32 b2Jacobian::Compute(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2) -{ - return b2Dot(linearA, x1) + angularA * a1 + b2Dot(linearB, x2) + angularB * a2; -} - inline b2JointType b2Joint::GetType() const { return m_type; @@ -213,6 +193,11 @@ inline b2Joint* b2Joint::GetNext() return m_next; } +inline const b2Joint* b2Joint::GetNext() const +{ + return m_next; +} + inline void* b2Joint::GetUserData() const { return m_userData; @@ -223,4 +208,9 @@ inline void b2Joint::SetUserData(void* data) m_userData = data; } +inline bool b2Joint::GetCollideConnected() const +{ + return m_collideConnected; +} + #endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2LineJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2LineJoint.cpp deleted file mode 100644 index b6b0a1cb2..000000000 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2LineJoint.cpp +++ /dev/null @@ -1,591 +0,0 @@ -/* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include - -// Linear constraint (point-to-line) -// d = p2 - p1 = x2 + r2 - x1 - r1 -// C = dot(perp, d) -// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1)) -// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2) -// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)] -// -// K = J * invM * JT -// -// J = [-a -s1 a s2] -// a = perp -// s1 = cross(d + r1, a) = cross(p2 - x1, a) -// s2 = cross(r2, a) = cross(p2 - x2, a) - - -// Motor/Limit linear constraint -// C = dot(ax1, d) -// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2) -// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)] - -// Block Solver -// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even -// when the mass has poor distribution (leading to large torques about the joint anchor points). -// -// The Jacobian has 3 rows: -// J = [-uT -s1 uT s2] // linear -// [-vT -a1 vT a2] // limit -// -// u = perp -// v = axis -// s1 = cross(d + r1, u), s2 = cross(r2, u) -// a1 = cross(d + r1, v), a2 = cross(r2, v) - -// M * (v2 - v1) = JT * df -// J * v2 = bias -// -// v2 = v1 + invM * JT * df -// J * (v1 + invM * JT * df) = bias -// K * df = bias - J * v1 = -Cdot -// K = J * invM * JT -// Cdot = J * v1 - bias -// -// Now solve for f2. -// df = f2 - f1 -// K * (f2 - f1) = -Cdot -// f2 = invK * (-Cdot) + f1 -// -// Clamp accumulated limit impulse. -// lower: f2(2) = max(f2(2), 0) -// upper: f2(2) = min(f2(2), 0) -// -// Solve for correct f2(1) -// K(1,1) * f2(1) = -Cdot(1) - K(1,2) * f2(2) + K(1,1:2) * f1 -// = -Cdot(1) - K(1,2) * f2(2) + K(1,1) * f1(1) + K(1,2) * f1(2) -// K(1,1) * f2(1) = -Cdot(1) - K(1,2) * (f2(2) - f1(2)) + K(1,1) * f1(1) -// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1) -// -// Now compute impulse to be applied: -// df = f2 - f1 - -void b2LineJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor, const b2Vec2& axis) -{ - bodyA = b1; - bodyB = b2; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - localAxisA = bodyA->GetLocalVector(axis); -} - -b2LineJoint::b2LineJoint(const b2LineJointDef* def) -: b2Joint(def) -{ - m_localAnchor1 = def->localAnchorA; - m_localAnchor2 = def->localAnchorB; - m_localXAxis1 = def->localAxisA; - m_localYAxis1 = b2Cross(1.0f, m_localXAxis1); - - m_impulse.SetZero(); - m_motorMass = 0.0; - m_motorImpulse = 0.0f; - - m_lowerTranslation = def->lowerTranslation; - m_upperTranslation = def->upperTranslation; - m_maxMotorForce = def->maxMotorForce; - m_motorSpeed = def->motorSpeed; - m_enableLimit = def->enableLimit; - m_enableMotor = def->enableMotor; - m_limitState = e_inactiveLimit; - - m_axis.SetZero(); - m_perp.SetZero(); -} - -void b2LineJoint::InitVelocityConstraints(const b2TimeStep& step) -{ - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; - - m_localCenterA = b1->GetLocalCenter(); - m_localCenterB = b2->GetLocalCenter(); - - b2Transform xf1 = b1->GetTransform(); - b2Transform xf2 = b2->GetTransform(); - - // Compute the effective masses. - b2Vec2 r1 = b2Mul(xf1.R, m_localAnchor1 - m_localCenterA); - b2Vec2 r2 = b2Mul(xf2.R, m_localAnchor2 - m_localCenterB); - b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1; - - m_invMassA = b1->m_invMass; - m_invIA = b1->m_invI; - m_invMassB = b2->m_invMass; - m_invIB = b2->m_invI; - - // Compute motor Jacobian and effective mass. - { - m_axis = b2Mul(xf1.R, m_localXAxis1); - m_a1 = b2Cross(d + r1, m_axis); - m_a2 = b2Cross(r2, m_axis); - - m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2; - if (m_motorMass > b2_epsilon) - { - m_motorMass = 1.0f / m_motorMass; - } - else - { - m_motorMass = 0.0f; - } - } - - // Prismatic constraint. - { - m_perp = b2Mul(xf1.R, m_localYAxis1); - - m_s1 = b2Cross(d + r1, m_perp); - m_s2 = b2Cross(r2, m_perp); - - float32 m1 = m_invMassA, m2 = m_invMassB; - float32 i1 = m_invIA, i2 = m_invIB; - - float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2; - float32 k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2; - float32 k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2; - - m_K.col1.Set(k11, k12); - m_K.col2.Set(k12, k22); - } - - // Compute motor and limit terms. - if (m_enableLimit) - { - float32 jointTranslation = b2Dot(m_axis, d); - if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) - { - m_limitState = e_equalLimits; - } - else if (jointTranslation <= m_lowerTranslation) - { - if (m_limitState != e_atLowerLimit) - { - m_limitState = e_atLowerLimit; - m_impulse.y = 0.0f; - } - } - else if (jointTranslation >= m_upperTranslation) - { - if (m_limitState != e_atUpperLimit) - { - m_limitState = e_atUpperLimit; - m_impulse.y = 0.0f; - } - } - else - { - m_limitState = e_inactiveLimit; - m_impulse.y = 0.0f; - } - } - else - { - m_limitState = e_inactiveLimit; - } - - if (m_enableMotor == false) - { - m_motorImpulse = 0.0f; - } - - if (step.warmStarting) - { - // Account for variable time step. - m_impulse *= step.dtRatio; - m_motorImpulse *= step.dtRatio; - - b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.y) * m_axis; - float32 L1 = m_impulse.x * m_s1 + (m_motorImpulse + m_impulse.y) * m_a1; - float32 L2 = m_impulse.x * m_s2 + (m_motorImpulse + m_impulse.y) * m_a2; - - b1->m_linearVelocity -= m_invMassA * P; - b1->m_angularVelocity -= m_invIA * L1; - - b2->m_linearVelocity += m_invMassB * P; - b2->m_angularVelocity += m_invIB * L2; - } - else - { - m_impulse.SetZero(); - m_motorImpulse = 0.0f; - } -} - -void b2LineJoint::SolveVelocityConstraints(const b2TimeStep& step) -{ - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; - - b2Vec2 v1 = b1->m_linearVelocity; - float32 w1 = b1->m_angularVelocity; - b2Vec2 v2 = b2->m_linearVelocity; - float32 w2 = b2->m_angularVelocity; - - // Solve linear motor constraint. - if (m_enableMotor && m_limitState != e_equalLimits) - { - float32 Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1; - float32 impulse = m_motorMass * (m_motorSpeed - Cdot); - float32 oldImpulse = m_motorImpulse; - float32 maxImpulse = step.dt * m_maxMotorForce; - m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_motorImpulse - oldImpulse; - - b2Vec2 P = impulse * m_axis; - float32 L1 = impulse * m_a1; - float32 L2 = impulse * m_a2; - - v1 -= m_invMassA * P; - w1 -= m_invIA * L1; - - v2 += m_invMassB * P; - w2 += m_invIB * L2; - } - - float32 Cdot1 = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1; - - if (m_enableLimit && m_limitState != e_inactiveLimit) - { - // Solve prismatic and limit constraint in block form. - float32 Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1; - b2Vec2 Cdot(Cdot1, Cdot2); - - b2Vec2 f1 = m_impulse; - b2Vec2 df = m_K.Solve(-Cdot); - m_impulse += df; - - if (m_limitState == e_atLowerLimit) - { - m_impulse.y = b2Max(m_impulse.y, 0.0f); - } - else if (m_limitState == e_atUpperLimit) - { - m_impulse.y = b2Min(m_impulse.y, 0.0f); - } - - // f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1) - float32 b = -Cdot1 - (m_impulse.y - f1.y) * m_K.col2.x; - float32 f2r; - if (m_K.col1.x != 0.0f) - { - f2r = b / m_K.col1.x + f1.x; - } - else - { - f2r = f1.x; - } - - m_impulse.x = f2r; - - df = m_impulse - f1; - - b2Vec2 P = df.x * m_perp + df.y * m_axis; - float32 L1 = df.x * m_s1 + df.y * m_a1; - float32 L2 = df.x * m_s2 + df.y * m_a2; - - v1 -= m_invMassA * P; - w1 -= m_invIA * L1; - - v2 += m_invMassB * P; - w2 += m_invIB * L2; - } - else - { - // Limit is inactive, just solve the prismatic constraint in block form. - float32 df; - if (m_K.col1.x != 0.0f) - { - df = - Cdot1 / m_K.col1.x; - } - else - { - df = 0.0f; - } - m_impulse.x += df; - - b2Vec2 P = df * m_perp; - float32 L1 = df * m_s1; - float32 L2 = df * m_s2; - - v1 -= m_invMassA * P; - w1 -= m_invIA * L1; - - v2 += m_invMassB * P; - w2 += m_invIB * L2; - } - - b1->m_linearVelocity = v1; - b1->m_angularVelocity = w1; - b2->m_linearVelocity = v2; - b2->m_angularVelocity = w2; -} - -bool b2LineJoint::SolvePositionConstraints(float32 baumgarte) -{ - B2_NOT_USED(baumgarte); - - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; - - b2Vec2 c1 = b1->m_sweep.c; - float32 a1 = b1->m_sweep.a; - - b2Vec2 c2 = b2->m_sweep.c; - float32 a2 = b2->m_sweep.a; - - // Solve linear limit constraint. - float32 linearError = 0.0f, angularError = 0.0f; - bool active = false; - float32 C2 = 0.0f; - - b2Mat22 R1(a1), R2(a2); - - b2Vec2 r1 = b2Mul(R1, m_localAnchor1 - m_localCenterA); - b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenterB); - b2Vec2 d = c2 + r2 - c1 - r1; - - if (m_enableLimit) - { - m_axis = b2Mul(R1, m_localXAxis1); - - m_a1 = b2Cross(d + r1, m_axis); - m_a2 = b2Cross(r2, m_axis); - - float32 translation = b2Dot(m_axis, d); - if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) - { - // Prevent large angular corrections - C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection); - linearError = b2Abs(translation); - active = true; - } - else if (translation <= m_lowerTranslation) - { - // Prevent large linear corrections and allow some slop. - C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); - linearError = m_lowerTranslation - translation; - active = true; - } - else if (translation >= m_upperTranslation) - { - // Prevent large linear corrections and allow some slop. - C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection); - linearError = translation - m_upperTranslation; - active = true; - } - } - - m_perp = b2Mul(R1, m_localYAxis1); - - m_s1 = b2Cross(d + r1, m_perp); - m_s2 = b2Cross(r2, m_perp); - - b2Vec2 impulse; - float32 C1; - C1 = b2Dot(m_perp, d); - - linearError = b2Max(linearError, b2Abs(C1)); - angularError = 0.0f; - - if (active) - { - float32 m1 = m_invMassA, m2 = m_invMassB; - float32 i1 = m_invIA, i2 = m_invIB; - - float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2; - float32 k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2; - float32 k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2; - - m_K.col1.Set(k11, k12); - m_K.col2.Set(k12, k22); - - b2Vec2 C; - C.x = C1; - C.y = C2; - - impulse = m_K.Solve(-C); - } - else - { - float32 m1 = m_invMassA, m2 = m_invMassB; - float32 i1 = m_invIA, i2 = m_invIB; - - float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2; - - float32 impulse1; - if (k11 != 0.0f) - { - impulse1 = - C1 / k11; - } - else - { - impulse1 = 0.0f; - } - - impulse.x = impulse1; - impulse.y = 0.0f; - } - - b2Vec2 P = impulse.x * m_perp + impulse.y * m_axis; - float32 L1 = impulse.x * m_s1 + impulse.y * m_a1; - float32 L2 = impulse.x * m_s2 + impulse.y * m_a2; - - c1 -= m_invMassA * P; - a1 -= m_invIA * L1; - c2 += m_invMassB * P; - a2 += m_invIB * L2; - - // TODO_ERIN remove need for this. - b1->m_sweep.c = c1; - b1->m_sweep.a = a1; - b2->m_sweep.c = c2; - b2->m_sweep.a = a2; - b1->SynchronizeTransform(); - b2->SynchronizeTransform(); - - return linearError <= b2_linearSlop && angularError <= b2_angularSlop; -} - -b2Vec2 b2LineJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchor1); -} - -b2Vec2 b2LineJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchor2); -} - -b2Vec2 b2LineJoint::GetReactionForce(float32 inv_dt) const -{ - return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.y) * m_axis); -} - -float32 b2LineJoint::GetReactionTorque(float32 inv_dt) const -{ - B2_NOT_USED(inv_dt); - return 0.0f; -} - -float32 b2LineJoint::GetJointTranslation() const -{ - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; - - b2Vec2 p1 = b1->GetWorldPoint(m_localAnchor1); - b2Vec2 p2 = b2->GetWorldPoint(m_localAnchor2); - b2Vec2 d = p2 - p1; - b2Vec2 axis = b1->GetWorldVector(m_localXAxis1); - - float32 translation = b2Dot(d, axis); - return translation; -} - -float32 b2LineJoint::GetJointSpeed() const -{ - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; - - b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); - b2Vec2 p1 = b1->m_sweep.c + r1; - b2Vec2 p2 = b2->m_sweep.c + r2; - b2Vec2 d = p2 - p1; - b2Vec2 axis = b1->GetWorldVector(m_localXAxis1); - - b2Vec2 v1 = b1->m_linearVelocity; - b2Vec2 v2 = b2->m_linearVelocity; - float32 w1 = b1->m_angularVelocity; - float32 w2 = b2->m_angularVelocity; - - float32 speed = b2Dot(d, b2Cross(w1, axis)) + b2Dot(axis, v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1)); - return speed; -} - -bool b2LineJoint::IsLimitEnabled() const -{ - return m_enableLimit; -} - -void b2LineJoint::EnableLimit(bool flag) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableLimit = flag; -} - -float32 b2LineJoint::GetLowerLimit() const -{ - return m_lowerTranslation; -} - -float32 b2LineJoint::GetUpperLimit() const -{ - return m_upperTranslation; -} - -void b2LineJoint::SetLimits(float32 lower, float32 upper) -{ - b2Assert(lower <= upper); - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_lowerTranslation = lower; - m_upperTranslation = upper; -} - -bool b2LineJoint::IsMotorEnabled() const -{ - return m_enableMotor; -} - -void b2LineJoint::EnableMotor(bool flag) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableMotor = flag; -} - -void b2LineJoint::SetMotorSpeed(float32 speed) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_motorSpeed = speed; -} - -void b2LineJoint::SetMaxMotorForce(float32 force) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_maxMotorForce = force; -} - -float32 b2LineJoint::GetMotorForce() const -{ - return m_motorImpulse; -} - - - - - diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2LineJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2LineJoint.h deleted file mode 100644 index 803e4c12d..000000000 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2LineJoint.h +++ /dev/null @@ -1,170 +0,0 @@ -/* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_LINE_JOINT_H -#define B2_LINE_JOINT_H - -#include - -/// Line joint definition. This requires defining a line of -/// motion using an axis and an anchor point. The definition uses local -/// anchor points and a local axis so that the initial configuration -/// can violate the constraint slightly. The joint translation is zero -/// when the local anchor points coincide in world space. Using local -/// anchors and a local axis helps when saving and loading a game. -struct b2LineJointDef : public b2JointDef -{ - b2LineJointDef() - { - type = e_lineJoint; - localAnchorA.SetZero(); - localAnchorB.SetZero(); - localAxisA.Set(1.0f, 0.0f); - enableLimit = false; - lowerTranslation = 0.0f; - upperTranslation = 0.0f; - enableMotor = false; - maxMotorForce = 0.0f; - motorSpeed = 0.0f; - } - - /// Initialize the bodies, anchors, axis, and reference angle using the world - /// anchor and world axis. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis); - - /// The local anchor point relative to body1's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to body2's origin. - b2Vec2 localAnchorB; - - /// The local translation axis in body1. - b2Vec2 localAxisA; - - /// Enable/disable the joint limit. - bool enableLimit; - - /// The lower translation limit, usually in meters. - float32 lowerTranslation; - - /// The upper translation limit, usually in meters. - float32 upperTranslation; - - /// Enable/disable the joint motor. - bool enableMotor; - - /// The maximum motor torque, usually in N-m. - float32 maxMotorForce; - - /// The desired motor speed in radians per second. - float32 motorSpeed; -}; - -/// A line joint. This joint provides two degrees of freedom: translation -/// along an axis fixed in body1 and rotation in the plane. You can use a -/// joint limit to restrict the range of motion and a joint motor to drive -/// the motion or to model joint friction. -class b2LineJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// Get the current joint translation, usually in meters. - float32 GetJointTranslation() const; - - /// Get the current joint translation speed, usually in meters per second. - float32 GetJointSpeed() const; - - /// Is the joint limit enabled? - bool IsLimitEnabled() const; - - /// Enable/disable the joint limit. - void EnableLimit(bool flag); - - /// Get the lower joint limit, usually in meters. - float32 GetLowerLimit() const; - - /// Get the upper joint limit, usually in meters. - float32 GetUpperLimit() const; - - /// Set the joint limits, usually in meters. - void SetLimits(float32 lower, float32 upper); - - /// Is the joint motor enabled? - bool IsMotorEnabled() const; - - /// Enable/disable the joint motor. - void EnableMotor(bool flag); - - /// Set the motor speed, usually in meters per second. - void SetMotorSpeed(float32 speed); - - /// Get the motor speed, usually in meters per second. - float32 GetMotorSpeed() const; - - /// Set/Get the maximum motor force, usually in N. - void SetMaxMotorForce(float32 force); - float32 GetMaxMotorForce() const; - - /// Get the current motor force, usually in N. - float32 GetMotorForce() const; - -protected: - - friend class b2Joint; - b2LineJoint(const b2LineJointDef* def); - - void InitVelocityConstraints(const b2TimeStep& step); - void SolveVelocityConstraints(const b2TimeStep& step); - bool SolvePositionConstraints(float32 baumgarte); - - b2Vec2 m_localAnchor1; - b2Vec2 m_localAnchor2; - b2Vec2 m_localXAxis1; - b2Vec2 m_localYAxis1; - - b2Vec2 m_axis, m_perp; - float32 m_s1, m_s2; - float32 m_a1, m_a2; - - b2Mat22 m_K; - b2Vec2 m_impulse; - - float32 m_motorMass; // effective mass for motor/limit translational constraint. - float32 m_motorImpulse; - - float32 m_lowerTranslation; - float32 m_upperTranslation; - float32 m_maxMotorForce; - float32 m_motorSpeed; - - bool m_enableLimit; - bool m_enableMotor; - b2LimitState m_limitState; -}; - -inline float32 b2LineJoint::GetMotorSpeed() const -{ - return m_motorSpeed; -} - -#endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2MouseJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2MouseJoint.cpp old mode 100644 new mode 100755 index b72ac5f30..be7c38348 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2MouseJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2MouseJoint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -36,8 +36,8 @@ b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def) b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f); b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f); - m_target = def->target; - m_localAnchor = b2MulT(m_bodyB->GetTransform(), m_target); + m_targetA = def->target; + m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA); m_maxForce = def->maxForce; m_impulse.SetZero(); @@ -55,12 +55,12 @@ void b2MouseJoint::SetTarget(const b2Vec2& target) { m_bodyB->SetAwake(true); } - m_target = target; + m_targetA = target; } const b2Vec2& b2MouseJoint::GetTarget() const { - return m_target; + return m_targetA; } void b2MouseJoint::SetMaxForce(float32 force) @@ -93,11 +93,21 @@ float32 b2MouseJoint::GetDampingRatio() const return m_dampingRatio; } -void b2MouseJoint::InitVelocityConstraints(const b2TimeStep& step) +void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data) { - b2Body* b = m_bodyB; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassB = m_bodyB->m_invMass; + m_invIB = m_bodyB->m_invI; - float32 mass = b->GetMass(); + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qB(aB); + + float32 mass = m_bodyB->GetMass(); // Frequency float32 omega = 2.0f * b2_pi * m_frequencyHz; @@ -111,79 +121,89 @@ void b2MouseJoint::InitVelocityConstraints(const b2TimeStep& step) // magic formulas // gamma has units of inverse mass. // beta has units of inverse time. - b2Assert(d + step.dt * k > b2_epsilon); - m_gamma = step.dt * (d + step.dt * k); + float32 h = data.step.dt; + b2Assert(d + h * k > b2_epsilon); + m_gamma = h * (d + h * k); if (m_gamma != 0.0f) { m_gamma = 1.0f / m_gamma; } - m_beta = step.dt * k * m_gamma; + m_beta = h * k * m_gamma; // Compute the effective mass matrix. - b2Vec2 r = b2Mul(b->GetTransform().R, m_localAnchor - b->GetLocalCenter()); + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] - float32 invMass = b->m_invMass; - float32 invI = b->m_invI; - - b2Mat22 K1; - K1.col1.x = invMass; K1.col2.x = 0.0f; - K1.col1.y = 0.0f; K1.col2.y = invMass; - - b2Mat22 K2; - K2.col1.x = invI * r.y * r.y; K2.col2.x = -invI * r.x * r.y; - K2.col1.y = -invI * r.x * r.y; K2.col2.y = invI * r.x * r.x; - - b2Mat22 K = K1 + K2; - K.col1.x += m_gamma; - K.col2.y += m_gamma; + b2Mat22 K; + K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma; + K.ex.y = -m_invIB * m_rB.x * m_rB.y; + K.ey.x = K.ex.y; + K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma; m_mass = K.GetInverse(); - m_C = b->m_sweep.c + r - m_target; + m_C = cB + m_rB - m_targetA; + m_C *= m_beta; // Cheat with some damping - b->m_angularVelocity *= 0.98f; + wB *= 0.98f; - // Warm starting. - m_impulse *= step.dtRatio; - b->m_linearVelocity += invMass * m_impulse; - b->m_angularVelocity += invI * b2Cross(r, m_impulse); + if (data.step.warmStarting) + { + m_impulse *= data.step.dtRatio; + vB += m_invMassB * m_impulse; + wB += m_invIB * b2Cross(m_rB, m_impulse); + } + else + { + m_impulse.SetZero(); + } + + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2MouseJoint::SolveVelocityConstraints(const b2TimeStep& step) +void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data) { - b2Body* b = m_bodyB; - - b2Vec2 r = b2Mul(b->GetTransform().R, m_localAnchor - b->GetLocalCenter()); + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; // Cdot = v + cross(w, r) - b2Vec2 Cdot = b->m_linearVelocity + b2Cross(b->m_angularVelocity, r); - b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_beta * m_C + m_gamma * m_impulse)); + b2Vec2 Cdot = vB + b2Cross(wB, m_rB); + b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse)); b2Vec2 oldImpulse = m_impulse; m_impulse += impulse; - float32 maxImpulse = step.dt * m_maxForce; + float32 maxImpulse = data.step.dt * m_maxForce; if (m_impulse.LengthSquared() > maxImpulse * maxImpulse) { m_impulse *= maxImpulse / m_impulse.Length(); } impulse = m_impulse - oldImpulse; - b->m_linearVelocity += b->m_invMass * impulse; - b->m_angularVelocity += b->m_invI * b2Cross(r, impulse); + vB += m_invMassB * impulse; + wB += m_invIB * b2Cross(m_rB, impulse); + + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; +} + +bool b2MouseJoint::SolvePositionConstraints(const b2SolverData& data) +{ + B2_NOT_USED(data); + return true; } b2Vec2 b2MouseJoint::GetAnchorA() const { - return m_target; + return m_targetA; } b2Vec2 b2MouseJoint::GetAnchorB() const { - return m_bodyB->GetWorldPoint(m_localAnchor); + return m_bodyB->GetWorldPoint(m_localAnchorB); } b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2MouseJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2MouseJoint.h old mode 100644 new mode 100755 index cd1959a42..3c90d0161 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2MouseJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2MouseJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -94,21 +94,30 @@ protected: b2MouseJoint(const b2MouseJointDef* def); - void InitVelocityConstraints(const b2TimeStep& step); - void SolveVelocityConstraints(const b2TimeStep& step); - bool SolvePositionConstraints(float32 baumgarte) { B2_NOT_USED(baumgarte); return true; } + void InitVelocityConstraints(const b2SolverData& data); + void SolveVelocityConstraints(const b2SolverData& data); + bool SolvePositionConstraints(const b2SolverData& data); - b2Vec2 m_localAnchor; - b2Vec2 m_target; - b2Vec2 m_impulse; - - b2Mat22 m_mass; // effective mass for point-to-point constraint. - b2Vec2 m_C; // position error - float32 m_maxForce; + b2Vec2 m_localAnchorB; + b2Vec2 m_targetA; float32 m_frequencyHz; float32 m_dampingRatio; float32 m_beta; + + // Solver shared + b2Vec2 m_impulse; + float32 m_maxForce; float32 m_gamma; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_rB; + b2Vec2 m_localCenterB; + float32 m_invMassB; + float32 m_invIB; + b2Mat22 m_mass; + b2Vec2 m_C; }; #endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp old mode 100644 new mode 100755 index a019888a4..99f0c7d6f --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -87,23 +87,23 @@ // Now compute impulse to be applied: // df = f2 - f1 -void b2PrismaticJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor, const b2Vec2& axis) +void b2PrismaticJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis) { - bodyA = b1; - bodyB = b2; + bodyA = bA; + bodyB = bB; localAnchorA = bodyA->GetLocalPoint(anchor); localAnchorB = bodyB->GetLocalPoint(anchor); - localAxis1 = bodyA->GetLocalVector(axis); + localAxisA = bodyA->GetLocalVector(axis); referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); } b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def) : b2Joint(def) { - m_localAnchor1 = def->localAnchorA; - m_localAnchor2 = def->localAnchorB; - m_localXAxis1 = def->localAxis1; - m_localYAxis1 = b2Cross(1.0f, m_localXAxis1); + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; + m_localXAxisA = def->localAxisA; + m_localYAxisA = b2Cross(1.0f, m_localXAxisA); m_refAngle = def->referenceAngle; m_impulse.SetZero(); @@ -122,35 +122,45 @@ b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def) m_perp.SetZero(); } -void b2PrismaticJoint::InitVelocityConstraints(const b2TimeStep& step) +void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data) { - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; - m_localCenterA = b1->GetLocalCenter(); - m_localCenterB = b2->GetLocalCenter(); + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; - b2Transform xf1 = b1->GetTransform(); - b2Transform xf2 = b2->GetTransform(); + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); // Compute the effective masses. - b2Vec2 r1 = b2Mul(xf1.R, m_localAnchor1 - m_localCenterA); - b2Vec2 r2 = b2Mul(xf2.R, m_localAnchor2 - m_localCenterB); - b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1; + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + b2Vec2 d = (cB - cA) + rB - rA; - m_invMassA = b1->m_invMass; - m_invIA = b1->m_invI; - m_invMassB = b2->m_invMass; - m_invIB = b2->m_invI; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; // Compute motor Jacobian and effective mass. { - m_axis = b2Mul(xf1.R, m_localXAxis1); - m_a1 = b2Cross(d + r1, m_axis); - m_a2 = b2Cross(r2, m_axis); + m_axis = b2Mul(qA, m_localXAxisA); + m_a1 = b2Cross(d + rA, m_axis); + m_a2 = b2Cross(rB, m_axis); - m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2; - if (m_motorMass > b2_epsilon) + m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; + if (m_motorMass > 0.0f) { m_motorMass = 1.0f / m_motorMass; } @@ -158,24 +168,26 @@ void b2PrismaticJoint::InitVelocityConstraints(const b2TimeStep& step) // Prismatic constraint. { - m_perp = b2Mul(xf1.R, m_localYAxis1); + m_perp = b2Mul(qA, m_localYAxisA); - m_s1 = b2Cross(d + r1, m_perp); - m_s2 = b2Cross(r2, m_perp); + m_s1 = b2Cross(d + rA, m_perp); + m_s2 = b2Cross(rB, m_perp); - float32 m1 = m_invMassA, m2 = m_invMassB; - float32 i1 = m_invIA, i2 = m_invIB; + float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2; + float32 k12 = iA * m_s1 + iB * m_s2; + float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2; + float32 k22 = iA + iB; + if (k22 == 0.0f) + { + // For bodies with fixed rotation. + k22 = 1.0f; + } + float32 k23 = iA * m_a1 + iB * m_a2; + float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; - float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2; - float32 k12 = i1 * m_s1 + i2 * m_s2; - float32 k13 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2; - float32 k22 = i1 + i2; - float32 k23 = i1 * m_a1 + i2 * m_a2; - float32 k33 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2; - - m_K.col1.Set(k11, k12, k13); - m_K.col2.Set(k12, k22, k23); - m_K.col3.Set(k13, k23, k33); + m_K.ex.Set(k11, k12, k13); + m_K.ey.Set(k12, k22, k23); + m_K.ez.Set(k13, k23, k33); } // Compute motor and limit terms. @@ -219,69 +231,74 @@ void b2PrismaticJoint::InitVelocityConstraints(const b2TimeStep& step) m_motorImpulse = 0.0f; } - if (step.warmStarting) + if (data.step.warmStarting) { // Account for variable time step. - m_impulse *= step.dtRatio; - m_motorImpulse *= step.dtRatio; + m_impulse *= data.step.dtRatio; + m_motorImpulse *= data.step.dtRatio; b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis; - float32 L1 = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1; - float32 L2 = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2; + float32 LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1; + float32 LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2; - b1->m_linearVelocity -= m_invMassA * P; - b1->m_angularVelocity -= m_invIA * L1; + vA -= mA * P; + wA -= iA * LA; - b2->m_linearVelocity += m_invMassB * P; - b2->m_angularVelocity += m_invIB * L2; + vB += mB * P; + wB += iB * LB; } else { m_impulse.SetZero(); m_motorImpulse = 0.0f; } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step) +void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data) { - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; - b2Vec2 v1 = b1->m_linearVelocity; - float32 w1 = b1->m_angularVelocity; - b2Vec2 v2 = b2->m_linearVelocity; - float32 w2 = b2->m_angularVelocity; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; // Solve linear motor constraint. if (m_enableMotor && m_limitState != e_equalLimits) { - float32 Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1; + float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; float32 impulse = m_motorMass * (m_motorSpeed - Cdot); float32 oldImpulse = m_motorImpulse; - float32 maxImpulse = step.dt * m_maxMotorForce; + float32 maxImpulse = data.step.dt * m_maxMotorForce; m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_motorImpulse - oldImpulse; b2Vec2 P = impulse * m_axis; - float32 L1 = impulse * m_a1; - float32 L2 = impulse * m_a2; + float32 LA = impulse * m_a1; + float32 LB = impulse * m_a2; - v1 -= m_invMassA * P; - w1 -= m_invIA * L1; + vA -= mA * P; + wA -= iA * LA; - v2 += m_invMassB * P; - w2 += m_invIB * L2; + vB += mB * P; + wB += iB * LB; } b2Vec2 Cdot1; - Cdot1.x = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1; - Cdot1.y = w2 - w1; + Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA; + Cdot1.y = wB - wA; if (m_enableLimit && m_limitState != e_inactiveLimit) { // Solve prismatic and limit constraint in block form. float32 Cdot2; - Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1; + Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); b2Vec3 f1 = m_impulse; @@ -298,7 +315,7 @@ void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step) } // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) - b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.col3.x, m_K.col3.y); + b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.ez.x, m_K.ez.y); b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y); m_impulse.x = f2r.x; m_impulse.y = f2r.y; @@ -306,14 +323,14 @@ void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step) df = m_impulse - f1; b2Vec2 P = df.x * m_perp + df.z * m_axis; - float32 L1 = df.x * m_s1 + df.y + df.z * m_a1; - float32 L2 = df.x * m_s2 + df.y + df.z * m_a2; + float32 LA = df.x * m_s1 + df.y + df.z * m_a1; + float32 LB = df.x * m_s2 + df.y + df.z * m_a2; - v1 -= m_invMassA * P; - w1 -= m_invIA * L1; + vA -= mA * P; + wA -= iA * LA; - v2 += m_invMassB * P; - w2 += m_invIB * L2; + vB += mB * P; + wB += iB * LB; } else { @@ -323,105 +340,100 @@ void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step) m_impulse.y += df.y; b2Vec2 P = df.x * m_perp; - float32 L1 = df.x * m_s1 + df.y; - float32 L2 = df.x * m_s2 + df.y; + float32 LA = df.x * m_s1 + df.y; + float32 LB = df.x * m_s2 + df.y; - v1 -= m_invMassA * P; - w1 -= m_invIA * L1; + vA -= mA * P; + wA -= iA * LA; - v2 += m_invMassB * P; - w2 += m_invIB * L2; + vB += mB * P; + wB += iB * LB; } - b1->m_linearVelocity = v1; - b1->m_angularVelocity = w1; - b2->m_linearVelocity = v2; - b2->m_angularVelocity = w2; + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -bool b2PrismaticJoint::SolvePositionConstraints(float32 baumgarte) +bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data) { - B2_NOT_USED(baumgarte); + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; + b2Rot qA(aA), qB(aB); - b2Vec2 c1 = b1->m_sweep.c; - float32 a1 = b1->m_sweep.a; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; - b2Vec2 c2 = b2->m_sweep.c; - float32 a2 = b2->m_sweep.a; + // Compute fresh Jacobians + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + b2Vec2 d = cB + rB - cA - rA; + + b2Vec2 axis = b2Mul(qA, m_localXAxisA); + float32 a1 = b2Cross(d + rA, axis); + float32 a2 = b2Cross(rB, axis); + b2Vec2 perp = b2Mul(qA, m_localYAxisA); + + float32 s1 = b2Cross(d + rA, perp); + float32 s2 = b2Cross(rB, perp); + + b2Vec3 impulse; + b2Vec2 C1; + C1.x = b2Dot(perp, d); + C1.y = aB - aA - m_refAngle; + + float32 linearError = b2Abs(C1.x); + float32 angularError = b2Abs(C1.y); - // Solve linear limit constraint. - float32 linearError = 0.0f, angularError = 0.0f; bool active = false; float32 C2 = 0.0f; - - b2Mat22 R1(a1), R2(a2); - - b2Vec2 r1 = b2Mul(R1, m_localAnchor1 - m_localCenterA); - b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenterB); - b2Vec2 d = c2 + r2 - c1 - r1; - if (m_enableLimit) { - m_axis = b2Mul(R1, m_localXAxis1); - - m_a1 = b2Cross(d + r1, m_axis); - m_a2 = b2Cross(r2, m_axis); - - float32 translation = b2Dot(m_axis, d); + float32 translation = b2Dot(axis, d); if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) { // Prevent large angular corrections C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection); - linearError = b2Abs(translation); + linearError = b2Max(linearError, b2Abs(translation)); active = true; } else if (translation <= m_lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); - linearError = m_lowerTranslation - translation; + linearError = b2Max(linearError, m_lowerTranslation - translation); active = true; } else if (translation >= m_upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection); - linearError = translation - m_upperTranslation; + linearError = b2Max(linearError, translation - m_upperTranslation); active = true; } } - m_perp = b2Mul(R1, m_localYAxis1); - - m_s1 = b2Cross(d + r1, m_perp); - m_s2 = b2Cross(r2, m_perp); - - b2Vec3 impulse; - b2Vec2 C1; - C1.x = b2Dot(m_perp, d); - C1.y = a2 - a1 - m_refAngle; - - linearError = b2Max(linearError, b2Abs(C1.x)); - angularError = b2Abs(C1.y); - if (active) { - float32 m1 = m_invMassA, m2 = m_invMassB; - float32 i1 = m_invIA, i2 = m_invIB; + float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2; + float32 k12 = iA * m_s1 + iB * m_s2; + float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2; + float32 k22 = iA + iB; + if (k22 == 0.0f) + { + // For fixed rotation + k22 = 1.0f; + } + float32 k23 = iA * m_a1 + iB * m_a2; + float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; - float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2; - float32 k12 = i1 * m_s1 + i2 * m_s2; - float32 k13 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2; - float32 k22 = i1 + i2; - float32 k23 = i1 * m_a1 + i2 * m_a2; - float32 k33 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2; - - m_K.col1.Set(k11, k12, k13); - m_K.col2.Set(k12, k22, k23); - m_K.col3.Set(k13, k23, k33); + m_K.ex.Set(k11, k12, k13); + m_K.ey.Set(k12, k22, k23); + m_K.ez.Set(k13, k23, k33); b2Vec3 C; C.x = C1.x; @@ -432,15 +444,16 @@ bool b2PrismaticJoint::SolvePositionConstraints(float32 baumgarte) } else { - float32 m1 = m_invMassA, m2 = m_invMassB; - float32 i1 = m_invIA, i2 = m_invIB; + float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2; + float32 k12 = iA * m_s1 + iB * m_s2; + float32 k22 = iA + iB; + if (k22 == 0.0f) + { + k22 = 1.0f; + } - float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2; - float32 k12 = i1 * m_s1 + i2 * m_s2; - float32 k22 = i1 + i2; - - m_K.col1.Set(k11, k12, 0.0f); - m_K.col2.Set(k12, k22, 0.0f); + m_K.ex.Set(k11, k12, 0.0f); + m_K.ey.Set(k12, k22, 0.0f); b2Vec2 impulse1 = m_K.Solve22(-C1); impulse.x = impulse1.x; @@ -448,34 +461,31 @@ bool b2PrismaticJoint::SolvePositionConstraints(float32 baumgarte) impulse.z = 0.0f; } - b2Vec2 P = impulse.x * m_perp + impulse.z * m_axis; - float32 L1 = impulse.x * m_s1 + impulse.y + impulse.z * m_a1; - float32 L2 = impulse.x * m_s2 + impulse.y + impulse.z * m_a2; + b2Vec2 P = impulse.x * perp + impulse.z * axis; + float32 LA = impulse.x * s1 + impulse.y + impulse.z * a1; + float32 LB = impulse.x * s2 + impulse.y + impulse.z * a2; - c1 -= m_invMassA * P; - a1 -= m_invIA * L1; - c2 += m_invMassB * P; - a2 += m_invIB * L2; + cA -= mA * P; + aA -= iA * LA; + cB += mB * P; + aB += iB * LB; + + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; - // TODO_ERIN remove need for this. - b1->m_sweep.c = c1; - b1->m_sweep.a = a1; - b2->m_sweep.c = c2; - b2->m_sweep.a = a2; - b1->SynchronizeTransform(); - b2->SynchronizeTransform(); - return linearError <= b2_linearSlop && angularError <= b2_angularSlop; } b2Vec2 b2PrismaticJoint::GetAnchorA() const { - return m_bodyA->GetWorldPoint(m_localAnchor1); + return m_bodyA->GetWorldPoint(m_localAnchorA); } b2Vec2 b2PrismaticJoint::GetAnchorB() const { - return m_bodyB->GetWorldPoint(m_localAnchor2); + return m_bodyB->GetWorldPoint(m_localAnchorB); } b2Vec2 b2PrismaticJoint::GetReactionForce(float32 inv_dt) const @@ -490,13 +500,10 @@ float32 b2PrismaticJoint::GetReactionTorque(float32 inv_dt) const float32 b2PrismaticJoint::GetJointTranslation() const { - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; - - b2Vec2 p1 = b1->GetWorldPoint(m_localAnchor1); - b2Vec2 p2 = b2->GetWorldPoint(m_localAnchor2); - b2Vec2 d = p2 - p1; - b2Vec2 axis = b1->GetWorldVector(m_localXAxis1); + b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA); + b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB); + b2Vec2 d = pB - pA; + b2Vec2 axis = m_bodyA->GetWorldVector(m_localXAxisA); float32 translation = b2Dot(d, axis); return translation; @@ -504,22 +511,22 @@ float32 b2PrismaticJoint::GetJointTranslation() const float32 b2PrismaticJoint::GetJointSpeed() const { - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; - b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); - b2Vec2 p1 = b1->m_sweep.c + r1; - b2Vec2 p2 = b2->m_sweep.c + r2; + b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter); + b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter); + b2Vec2 p1 = bA->m_sweep.c + rA; + b2Vec2 p2 = bB->m_sweep.c + rB; b2Vec2 d = p2 - p1; - b2Vec2 axis = b1->GetWorldVector(m_localXAxis1); + b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA); - b2Vec2 v1 = b1->m_linearVelocity; - b2Vec2 v2 = b2->m_linearVelocity; - float32 w1 = b1->m_angularVelocity; - float32 w2 = b2->m_angularVelocity; + b2Vec2 vA = bA->m_linearVelocity; + b2Vec2 vB = bB->m_linearVelocity; + float32 wA = bA->m_angularVelocity; + float32 wB = bB->m_angularVelocity; - float32 speed = b2Dot(d, b2Cross(w1, axis)) + b2Dot(axis, v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1)); + float32 speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA)); return speed; } @@ -530,9 +537,13 @@ bool b2PrismaticJoint::IsLimitEnabled() const void b2PrismaticJoint::EnableLimit(bool flag) { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableLimit = flag; + if (flag != m_enableLimit) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableLimit = flag; + m_impulse.z = 0.0f; + } } float32 b2PrismaticJoint::GetLowerLimit() const @@ -548,10 +559,14 @@ float32 b2PrismaticJoint::GetUpperLimit() const void b2PrismaticJoint::SetLimits(float32 lower, float32 upper) { b2Assert(lower <= upper); - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_lowerTranslation = lower; - m_upperTranslation = upper; + if (lower != m_lowerTranslation || upper != m_upperTranslation) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_lowerTranslation = lower; + m_upperTranslation = upper; + m_impulse.z = 0.0f; + } } bool b2PrismaticJoint::IsMotorEnabled() const @@ -580,7 +595,7 @@ void b2PrismaticJoint::SetMaxMotorForce(float32 force) m_maxMotorForce = force; } -float32 b2PrismaticJoint::GetMotorForce() const +float32 b2PrismaticJoint::GetMotorForce(float32 inv_dt) const { - return m_motorImpulse; + return inv_dt * m_motorImpulse; } diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.h old mode 100644 new mode 100755 index 7a12c5df8..f271e7579 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -27,7 +27,6 @@ /// can violate the constraint slightly. The joint translation is zero /// when the local anchor points coincide in world space. Using local /// anchors and a local axis helps when saving and loading a game. -/// @warning at least one body should by dynamic with a non-fixed rotation. struct b2PrismaticJointDef : public b2JointDef { b2PrismaticJointDef() @@ -35,7 +34,7 @@ struct b2PrismaticJointDef : public b2JointDef type = e_prismaticJoint; localAnchorA.SetZero(); localAnchorB.SetZero(); - localAxis1.Set(1.0f, 0.0f); + localAxisA.Set(1.0f, 0.0f); referenceAngle = 0.0f; enableLimit = false; lowerTranslation = 0.0f; @@ -56,9 +55,9 @@ struct b2PrismaticJointDef : public b2JointDef b2Vec2 localAnchorB; /// The local translation axis in body1. - b2Vec2 localAxis1; + b2Vec2 localAxisA; - /// The constrained angle between the bodies: body2_angle - body1_angle. + /// The constrained angle between the bodies: bodyB_angle - bodyA_angle. float32 referenceAngle; /// Enable/disable the joint limit. @@ -81,7 +80,7 @@ struct b2PrismaticJointDef : public b2JointDef }; /// A prismatic joint. This joint provides one degree of freedom: translation -/// along an axis fixed in body1. Relative rotation is prevented. You can +/// along an axis fixed in bodyA. Relative rotation is prevented. You can /// use a joint limit to restrict the range of motion and a joint motor to /// drive the motion or to model joint friction. class b2PrismaticJoint : public b2Joint @@ -129,42 +128,48 @@ public: /// Set the maximum motor force, usually in N. void SetMaxMotorForce(float32 force); - /// Get the current motor force, usually in N. - float32 GetMotorForce() const; + /// Get the current motor force given the inverse time step, usually in N. + float32 GetMotorForce(float32 inv_dt) const; protected: friend class b2Joint; friend class b2GearJoint; b2PrismaticJoint(const b2PrismaticJointDef* def); - void InitVelocityConstraints(const b2TimeStep& step); - void SolveVelocityConstraints(const b2TimeStep& step); - bool SolvePositionConstraints(float32 baumgarte); + void InitVelocityConstraints(const b2SolverData& data); + void SolveVelocityConstraints(const b2SolverData& data); + bool SolvePositionConstraints(const b2SolverData& data); - b2Vec2 m_localAnchor1; - b2Vec2 m_localAnchor2; - b2Vec2 m_localXAxis1; - b2Vec2 m_localYAxis1; + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + b2Vec2 m_localXAxisA; + b2Vec2 m_localYAxisA; float32 m_refAngle; - - b2Vec2 m_axis, m_perp; - float32 m_s1, m_s2; - float32 m_a1, m_a2; - - b2Mat33 m_K; b2Vec3 m_impulse; - - float32 m_motorMass; // effective mass for motor/limit translational constraint. float32 m_motorImpulse; - float32 m_lowerTranslation; float32 m_upperTranslation; float32 m_maxMotorForce; float32 m_motorSpeed; - bool m_enableLimit; bool m_enableMotor; b2LimitState m_limitState; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + b2Vec2 m_axis, m_perp; + float32 m_s1, m_s2; + float32 m_a1, m_a2; + b2Mat33 m_K; + float32 m_motorMass; }; inline float32 b2PrismaticJoint::GetMotorSpeed() const diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.cpp old mode 100644 new mode 100755 index beb7db8fb..1a09d907e --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2007 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -24,368 +24,251 @@ // length1 = norm(p1 - s1) // length2 = norm(p2 - s2) // C0 = (length1 + ratio * length2)_initial -// C = C0 - (length1 + ratio * length2) >= 0 +// C = C0 - (length1 + ratio * length2) // u1 = (p1 - s1) / norm(p1 - s1) // u2 = (p2 - s2) / norm(p2 - s2) // Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2)) // J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)] // K = J * invM * JT // = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2) -// -// Limit: -// C = maxLength - length -// u = (p - s) / norm(p - s) -// Cdot = -dot(u, v + cross(w, r)) -// K = invMass + invI * cross(r, u)^2 -// 0 <= impulse -void b2PulleyJointDef::Initialize(b2Body* b1, b2Body* b2, - const b2Vec2& ga1, const b2Vec2& ga2, - const b2Vec2& anchor1, const b2Vec2& anchor2, +void b2PulleyJointDef::Initialize(b2Body* bA, b2Body* bB, + const b2Vec2& groundA, const b2Vec2& groundB, + const b2Vec2& anchorA, const b2Vec2& anchorB, float32 r) { - bodyA = b1; - bodyB = b2; - groundAnchorA = ga1; - groundAnchorB = ga2; - localAnchorA = bodyA->GetLocalPoint(anchor1); - localAnchorB = bodyB->GetLocalPoint(anchor2); - b2Vec2 d1 = anchor1 - ga1; - lengthA = d1.Length(); - b2Vec2 d2 = anchor2 - ga2; - lengthB = d2.Length(); + bodyA = bA; + bodyB = bB; + groundAnchorA = groundA; + groundAnchorB = groundB; + localAnchorA = bodyA->GetLocalPoint(anchorA); + localAnchorB = bodyB->GetLocalPoint(anchorB); + b2Vec2 dA = anchorA - groundA; + lengthA = dA.Length(); + b2Vec2 dB = anchorB - groundB; + lengthB = dB.Length(); ratio = r; b2Assert(ratio > b2_epsilon); - float32 C = lengthA + ratio * lengthB; - maxLengthA = C - ratio * b2_minPulleyLength; - maxLengthB = (C - b2_minPulleyLength) / ratio; } b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def) : b2Joint(def) { - m_groundAnchor1 = def->groundAnchorA; - m_groundAnchor2 = def->groundAnchorB; - m_localAnchor1 = def->localAnchorA; - m_localAnchor2 = def->localAnchorB; + m_groundAnchorA = def->groundAnchorA; + m_groundAnchorB = def->groundAnchorB; + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; b2Assert(def->ratio != 0.0f); m_ratio = def->ratio; m_constant = def->lengthA + m_ratio * def->lengthB; - m_maxLength1 = b2Min(def->maxLengthA, m_constant - m_ratio * b2_minPulleyLength); - m_maxLength2 = b2Min(def->maxLengthB, (m_constant - b2_minPulleyLength) / m_ratio); - m_impulse = 0.0f; - m_limitImpulse1 = 0.0f; - m_limitImpulse2 = 0.0f; } -void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step) +void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data) { - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; - b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; - b2Vec2 p1 = b1->m_sweep.c + r1; - b2Vec2 p2 = b2->m_sweep.c + r2; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; - b2Vec2 s1 = m_groundAnchor1; - b2Vec2 s2 = m_groundAnchor2; + b2Rot qA(aA), qB(aB); + + m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); // Get the pulley axes. - m_u1 = p1 - s1; - m_u2 = p2 - s2; + m_uA = cA + m_rA - m_groundAnchorA; + m_uB = cB + m_rB - m_groundAnchorB; - float32 length1 = m_u1.Length(); - float32 length2 = m_u2.Length(); + float32 lengthA = m_uA.Length(); + float32 lengthB = m_uB.Length(); - if (length1 > b2_linearSlop) + if (lengthA > 10.0f * b2_linearSlop) { - m_u1 *= 1.0f / length1; + m_uA *= 1.0f / lengthA; } else { - m_u1.SetZero(); + m_uA.SetZero(); } - if (length2 > b2_linearSlop) + if (lengthB > 10.0f * b2_linearSlop) { - m_u2 *= 1.0f / length2; + m_uB *= 1.0f / lengthB; } else { - m_u2.SetZero(); - } - - float32 C = m_constant - length1 - m_ratio * length2; - if (C > 0.0f) - { - m_state = e_inactiveLimit; - m_impulse = 0.0f; - } - else - { - m_state = e_atUpperLimit; - } - - if (length1 < m_maxLength1) - { - m_limitState1 = e_inactiveLimit; - m_limitImpulse1 = 0.0f; - } - else - { - m_limitState1 = e_atUpperLimit; - } - - if (length2 < m_maxLength2) - { - m_limitState2 = e_inactiveLimit; - m_limitImpulse2 = 0.0f; - } - else - { - m_limitState2 = e_atUpperLimit; + m_uB.SetZero(); } // Compute effective mass. - float32 cr1u1 = b2Cross(r1, m_u1); - float32 cr2u2 = b2Cross(r2, m_u2); + float32 ruA = b2Cross(m_rA, m_uA); + float32 ruB = b2Cross(m_rB, m_uB); - m_limitMass1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1; - m_limitMass2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2; - m_pulleyMass = m_limitMass1 + m_ratio * m_ratio * m_limitMass2; - b2Assert(m_limitMass1 > b2_epsilon); - b2Assert(m_limitMass2 > b2_epsilon); - b2Assert(m_pulleyMass > b2_epsilon); - m_limitMass1 = 1.0f / m_limitMass1; - m_limitMass2 = 1.0f / m_limitMass2; - m_pulleyMass = 1.0f / m_pulleyMass; + float32 mA = m_invMassA + m_invIA * ruA * ruA; + float32 mB = m_invMassB + m_invIB * ruB * ruB; - if (step.warmStarting) + m_mass = mA + m_ratio * m_ratio * mB; + + if (m_mass > 0.0f) + { + m_mass = 1.0f / m_mass; + } + + if (data.step.warmStarting) { // Scale impulses to support variable time steps. - m_impulse *= step.dtRatio; - m_limitImpulse1 *= step.dtRatio; - m_limitImpulse2 *= step.dtRatio; + m_impulse *= data.step.dtRatio; // Warm starting. - b2Vec2 P1 = -(m_impulse + m_limitImpulse1) * m_u1; - b2Vec2 P2 = (-m_ratio * m_impulse - m_limitImpulse2) * m_u2; - b1->m_linearVelocity += b1->m_invMass * P1; - b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1); - b2->m_linearVelocity += b2->m_invMass * P2; - b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2); + b2Vec2 PA = -(m_impulse) * m_uA; + b2Vec2 PB = (-m_ratio * m_impulse) * m_uB; + + vA += m_invMassA * PA; + wA += m_invIA * b2Cross(m_rA, PA); + vB += m_invMassB * PB; + wB += m_invIB * b2Cross(m_rB, PB); } else { m_impulse = 0.0f; - m_limitImpulse1 = 0.0f; - m_limitImpulse2 = 0.0f; } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2PulleyJoint::SolveVelocityConstraints(const b2TimeStep& step) +void b2PulleyJoint::SolveVelocityConstraints(const b2SolverData& data) { - B2_NOT_USED(step); + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; + b2Vec2 vpA = vA + b2Cross(wA, m_rA); + b2Vec2 vpB = vB + b2Cross(wB, m_rB); - b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); + float32 Cdot = -b2Dot(m_uA, vpA) - m_ratio * b2Dot(m_uB, vpB); + float32 impulse = -m_mass * Cdot; + m_impulse += impulse; - if (m_state == e_atUpperLimit) - { - b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1); - b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2); + b2Vec2 PA = -impulse * m_uA; + b2Vec2 PB = -m_ratio * impulse * m_uB; + vA += m_invMassA * PA; + wA += m_invIA * b2Cross(m_rA, PA); + vB += m_invMassB * PB; + wB += m_invIB * b2Cross(m_rB, PB); - float32 Cdot = -b2Dot(m_u1, v1) - m_ratio * b2Dot(m_u2, v2); - float32 impulse = m_pulleyMass * (-Cdot); - float32 oldImpulse = m_impulse; - m_impulse = b2Max(0.0f, m_impulse + impulse); - impulse = m_impulse - oldImpulse; - - b2Vec2 P1 = -impulse * m_u1; - b2Vec2 P2 = -m_ratio * impulse * m_u2; - b1->m_linearVelocity += b1->m_invMass * P1; - b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1); - b2->m_linearVelocity += b2->m_invMass * P2; - b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2); - } - - if (m_limitState1 == e_atUpperLimit) - { - b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1); - - float32 Cdot = -b2Dot(m_u1, v1); - float32 impulse = -m_limitMass1 * Cdot; - float32 oldImpulse = m_limitImpulse1; - m_limitImpulse1 = b2Max(0.0f, m_limitImpulse1 + impulse); - impulse = m_limitImpulse1 - oldImpulse; - - b2Vec2 P1 = -impulse * m_u1; - b1->m_linearVelocity += b1->m_invMass * P1; - b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1); - } - - if (m_limitState2 == e_atUpperLimit) - { - b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2); - - float32 Cdot = -b2Dot(m_u2, v2); - float32 impulse = -m_limitMass2 * Cdot; - float32 oldImpulse = m_limitImpulse2; - m_limitImpulse2 = b2Max(0.0f, m_limitImpulse2 + impulse); - impulse = m_limitImpulse2 - oldImpulse; - - b2Vec2 P2 = -impulse * m_u2; - b2->m_linearVelocity += b2->m_invMass * P2; - b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2); - } + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -bool b2PulleyJoint::SolvePositionConstraints(float32 baumgarte) +bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data) { - B2_NOT_USED(baumgarte); + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; + b2Rot qA(aA), qB(aB); - b2Vec2 s1 = m_groundAnchor1; - b2Vec2 s2 = m_groundAnchor2; + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - float32 linearError = 0.0f; + // Get the pulley axes. + b2Vec2 uA = cA + rA - m_groundAnchorA; + b2Vec2 uB = cB + rB - m_groundAnchorB; - if (m_state == e_atUpperLimit) + float32 lengthA = uA.Length(); + float32 lengthB = uB.Length(); + + if (lengthA > 10.0f * b2_linearSlop) { - b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); - - b2Vec2 p1 = b1->m_sweep.c + r1; - b2Vec2 p2 = b2->m_sweep.c + r2; - - // Get the pulley axes. - m_u1 = p1 - s1; - m_u2 = p2 - s2; - - float32 length1 = m_u1.Length(); - float32 length2 = m_u2.Length(); - - if (length1 > b2_linearSlop) - { - m_u1 *= 1.0f / length1; - } - else - { - m_u1.SetZero(); - } - - if (length2 > b2_linearSlop) - { - m_u2 *= 1.0f / length2; - } - else - { - m_u2.SetZero(); - } - - float32 C = m_constant - length1 - m_ratio * length2; - linearError = b2Max(linearError, -C); - - C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); - float32 impulse = -m_pulleyMass * C; - - b2Vec2 P1 = -impulse * m_u1; - b2Vec2 P2 = -m_ratio * impulse * m_u2; - - b1->m_sweep.c += b1->m_invMass * P1; - b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1); - b2->m_sweep.c += b2->m_invMass * P2; - b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2); - - b1->SynchronizeTransform(); - b2->SynchronizeTransform(); + uA *= 1.0f / lengthA; + } + else + { + uA.SetZero(); } - if (m_limitState1 == e_atUpperLimit) + if (lengthB > 10.0f * b2_linearSlop) { - b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - b2Vec2 p1 = b1->m_sweep.c + r1; - - m_u1 = p1 - s1; - float32 length1 = m_u1.Length(); - - if (length1 > b2_linearSlop) - { - m_u1 *= 1.0f / length1; - } - else - { - m_u1.SetZero(); - } - - float32 C = m_maxLength1 - length1; - linearError = b2Max(linearError, -C); - C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); - float32 impulse = -m_limitMass1 * C; - - b2Vec2 P1 = -impulse * m_u1; - b1->m_sweep.c += b1->m_invMass * P1; - b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1); - - b1->SynchronizeTransform(); + uB *= 1.0f / lengthB; + } + else + { + uB.SetZero(); } - if (m_limitState2 == e_atUpperLimit) + // Compute effective mass. + float32 ruA = b2Cross(rA, uA); + float32 ruB = b2Cross(rB, uB); + + float32 mA = m_invMassA + m_invIA * ruA * ruA; + float32 mB = m_invMassB + m_invIB * ruB * ruB; + + float32 mass = mA + m_ratio * m_ratio * mB; + + if (mass > 0.0f) { - b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); - b2Vec2 p2 = b2->m_sweep.c + r2; - - m_u2 = p2 - s2; - float32 length2 = m_u2.Length(); - - if (length2 > b2_linearSlop) - { - m_u2 *= 1.0f / length2; - } - else - { - m_u2.SetZero(); - } - - float32 C = m_maxLength2 - length2; - linearError = b2Max(linearError, -C); - C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); - float32 impulse = -m_limitMass2 * C; - - b2Vec2 P2 = -impulse * m_u2; - b2->m_sweep.c += b2->m_invMass * P2; - b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2); - - b2->SynchronizeTransform(); + mass = 1.0f / mass; } + float32 C = m_constant - lengthA - m_ratio * lengthB; + float32 linearError = b2Abs(C); + + float32 impulse = -mass * C; + + b2Vec2 PA = -impulse * uA; + b2Vec2 PB = -m_ratio * impulse * uB; + + cA += m_invMassA * PA; + aA += m_invIA * b2Cross(rA, PA); + cB += m_invMassB * PB; + aB += m_invIB * b2Cross(rB, PB); + + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; + return linearError < b2_linearSlop; } b2Vec2 b2PulleyJoint::GetAnchorA() const { - return m_bodyA->GetWorldPoint(m_localAnchor1); + return m_bodyA->GetWorldPoint(m_localAnchorA); } b2Vec2 b2PulleyJoint::GetAnchorB() const { - return m_bodyB->GetWorldPoint(m_localAnchor2); + return m_bodyB->GetWorldPoint(m_localAnchorB); } b2Vec2 b2PulleyJoint::GetReactionForce(float32 inv_dt) const { - b2Vec2 P = m_impulse * m_u2; + b2Vec2 P = m_impulse * m_uB; return inv_dt * P; } @@ -397,26 +280,26 @@ float32 b2PulleyJoint::GetReactionTorque(float32 inv_dt) const b2Vec2 b2PulleyJoint::GetGroundAnchorA() const { - return m_groundAnchor1; + return m_groundAnchorA; } b2Vec2 b2PulleyJoint::GetGroundAnchorB() const { - return m_groundAnchor2; + return m_groundAnchorB; } -float32 b2PulleyJoint::GetLength1() const +float32 b2PulleyJoint::GetLengthA() const { - b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchor1); - b2Vec2 s = m_groundAnchor1; + b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA); + b2Vec2 s = m_groundAnchorA; b2Vec2 d = p - s; return d.Length(); } -float32 b2PulleyJoint::GetLength2() const +float32 b2PulleyJoint::GetLengthB() const { - b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchor2); - b2Vec2 s = m_groundAnchor2; + b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchorB); + b2Vec2 s = m_groundAnchorB; b2Vec2 d = p - s; return d.Length(); } diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.h old mode 100644 new mode 100755 index 189decb31..a5269c618 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -24,8 +24,7 @@ const float32 b2_minPulleyLength = 2.0f; /// Pulley joint definition. This requires two ground anchors, -/// two dynamic body anchor points, max lengths for each side, -/// and a pulley ratio. +/// two dynamic body anchor points, and a pulley ratio. struct b2PulleyJointDef : public b2JointDef { b2PulleyJointDef() @@ -36,9 +35,7 @@ struct b2PulleyJointDef : public b2JointDef localAnchorA.Set(-1.0f, 0.0f); localAnchorB.Set(1.0f, 0.0f); lengthA = 0.0f; - maxLengthA = 0.0f; lengthB = 0.0f; - maxLengthB = 0.0f; ratio = 1.0f; collideConnected = true; } @@ -64,15 +61,9 @@ struct b2PulleyJointDef : public b2JointDef /// The a reference length for the segment attached to bodyA. float32 lengthA; - /// The maximum length of the segment attached to bodyA. - float32 maxLengthA; - /// The a reference length for the segment attached to bodyB. float32 lengthB; - /// The maximum length of the segment attached to bodyB. - float32 maxLengthB; - /// The pulley ratio, used to simulate a block-and-tackle. float32 ratio; }; @@ -81,8 +72,10 @@ struct b2PulleyJointDef : public b2JointDef /// The pulley supports a ratio such that: /// length1 + ratio * length2 <= constant /// Yes, the force transmitted is scaled by the ratio. -/// The pulley also enforces a maximum length limit on both sides. This is -/// useful to prevent one side of the pulley hitting the top. +/// Warning: the pulley joint can get a bit squirrelly by itself. They often +/// work better when combined with prismatic joints. You should also cover the +/// the anchor points with static shapes to prevent one side from going to +/// zero length. class b2PulleyJoint : public b2Joint { public: @@ -98,11 +91,11 @@ public: /// Get the second ground anchor. b2Vec2 GetGroundAnchorB() const; - /// Get the current length of the segment attached to body1. - float32 GetLength1() const; + /// Get the current length of the segment attached to bodyA. + float32 GetLengthA() const; - /// Get the current length of the segment attached to body2. - float32 GetLength2() const; + /// Get the current length of the segment attached to bodyB. + float32 GetLengthB() const; /// Get the pulley ratio. float32 GetRatio() const; @@ -112,37 +105,34 @@ protected: friend class b2Joint; b2PulleyJoint(const b2PulleyJointDef* data); - void InitVelocityConstraints(const b2TimeStep& step); - void SolveVelocityConstraints(const b2TimeStep& step); - bool SolvePositionConstraints(float32 baumgarte); + void InitVelocityConstraints(const b2SolverData& data); + void SolveVelocityConstraints(const b2SolverData& data); + bool SolvePositionConstraints(const b2SolverData& data); - b2Vec2 m_groundAnchor1; - b2Vec2 m_groundAnchor2; - b2Vec2 m_localAnchor1; - b2Vec2 m_localAnchor2; - - b2Vec2 m_u1; - b2Vec2 m_u2; + b2Vec2 m_groundAnchorA; + b2Vec2 m_groundAnchorB; + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; float32 m_constant; float32 m_ratio; - - float32 m_maxLength1; - float32 m_maxLength2; - - // Effective masses - float32 m_pulleyMass; - float32 m_limitMass1; - float32 m_limitMass2; - - // Impulses for accumulation/warm starting. float32 m_impulse; - float32 m_limitImpulse1; - float32 m_limitImpulse2; - b2LimitState m_state; - b2LimitState m_limitState1; - b2LimitState m_limitState2; + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_uA; + b2Vec2 m_uB; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + float32 m_mass; }; #endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp old mode 100644 new mode 100755 index f9845265a..7718f03fc --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -33,10 +33,10 @@ // J = [0 0 -1 0 0 1] // K = invI1 + invI2 -void b2RevoluteJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor) +void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) { - bodyA = b1; - bodyB = b2; + bodyA = bA; + bodyB = bB; localAnchorA = bodyA->GetLocalPoint(anchor); localAnchorB = bodyB->GetLocalPoint(anchor); referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); @@ -45,8 +45,8 @@ void b2RevoluteJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def) : b2Joint(def) { - m_localAnchor1 = def->localAnchorA; - m_localAnchor2 = def->localAnchorB; + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; m_referenceAngle = def->referenceAngle; m_impulse.SetZero(); @@ -61,58 +61,70 @@ b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def) m_limitState = e_inactiveLimit; } -void b2RevoluteJoint::InitVelocityConstraints(const b2TimeStep& step) +void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data) { - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; - if (m_enableMotor || m_enableLimit) - { - // You cannot create a rotation limit between bodies that - // both have fixed rotation. - b2Assert(b1->m_invI > 0.0f || b2->m_invI > 0.0f); - } + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; - // Compute the effective mass matrix. - b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab - // K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2] - // [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2] - // [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2] + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - float32 m1 = b1->m_invMass, m2 = b2->m_invMass; - float32 i1 = b1->m_invI, i2 = b2->m_invI; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; - m_mass.col1.x = m1 + m2 + r1.y * r1.y * i1 + r2.y * r2.y * i2; - m_mass.col2.x = -r1.y * r1.x * i1 - r2.y * r2.x * i2; - m_mass.col3.x = -r1.y * i1 - r2.y * i2; - m_mass.col1.y = m_mass.col2.x; - m_mass.col2.y = m1 + m2 + r1.x * r1.x * i1 + r2.x * r2.x * i2; - m_mass.col3.y = r1.x * i1 + r2.x * i2; - m_mass.col1.z = m_mass.col3.x; - m_mass.col2.z = m_mass.col3.y; - m_mass.col3.z = i1 + i2; + bool fixedRotation = (iA + iB == 0.0f); - m_motorMass = i1 + i2; + m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; + m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; + m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB; + m_mass.ex.y = m_mass.ey.x; + m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; + m_mass.ez.y = m_rA.x * iA + m_rB.x * iB; + m_mass.ex.z = m_mass.ez.x; + m_mass.ey.z = m_mass.ez.y; + m_mass.ez.z = iA + iB; + + m_motorMass = iA + iB; if (m_motorMass > 0.0f) { m_motorMass = 1.0f / m_motorMass; } - if (m_enableMotor == false) + if (m_enableMotor == false || fixedRotation) { m_motorImpulse = 0.0f; } - if (m_enableLimit) + if (m_enableLimit && fixedRotation == false) { - float32 jointAngle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle; + float32 jointAngle = aB - aA - m_referenceAngle; if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop) { m_limitState = e_equalLimits; @@ -144,66 +156,66 @@ void b2RevoluteJoint::InitVelocityConstraints(const b2TimeStep& step) m_limitState = e_inactiveLimit; } - if (step.warmStarting) + if (data.step.warmStarting) { // Scale impulses to support a variable time step. - m_impulse *= step.dtRatio; - m_motorImpulse *= step.dtRatio; + m_impulse *= data.step.dtRatio; + m_motorImpulse *= data.step.dtRatio; b2Vec2 P(m_impulse.x, m_impulse.y); - b1->m_linearVelocity -= m1 * P; - b1->m_angularVelocity -= i1 * (b2Cross(r1, P) + m_motorImpulse + m_impulse.z); + vA -= mA * P; + wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z); - b2->m_linearVelocity += m2 * P; - b2->m_angularVelocity += i2 * (b2Cross(r2, P) + m_motorImpulse + m_impulse.z); + vB += mB * P; + wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z); } else { m_impulse.SetZero(); m_motorImpulse = 0.0f; } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step) +void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data) { - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; - b2Vec2 v1 = b1->m_linearVelocity; - float32 w1 = b1->m_angularVelocity; - b2Vec2 v2 = b2->m_linearVelocity; - float32 w2 = b2->m_angularVelocity; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; - float32 m1 = b1->m_invMass, m2 = b2->m_invMass; - float32 i1 = b1->m_invI, i2 = b2->m_invI; + bool fixedRotation = (iA + iB == 0.0f); // Solve motor constraint. - if (m_enableMotor && m_limitState != e_equalLimits) + if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false) { - float32 Cdot = w2 - w1 - m_motorSpeed; - float32 impulse = m_motorMass * (-Cdot); + float32 Cdot = wB - wA - m_motorSpeed; + float32 impulse = -m_motorMass * Cdot; float32 oldImpulse = m_motorImpulse; - float32 maxImpulse = step.dt * m_maxMotorTorque; + float32 maxImpulse = data.step.dt * m_maxMotorTorque; m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_motorImpulse - oldImpulse; - w1 -= i1 * impulse; - w2 += i2 * impulse; + wA -= iA * impulse; + wB += iB * impulse; } // Solve limit constraint. - if (m_enableLimit && m_limitState != e_inactiveLimit) + if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false) { - b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); - - // Solve point-to-point constraint - b2Vec2 Cdot1 = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1); - float32 Cdot2 = w2 - w1; + b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); + float32 Cdot2 = wB - wA; b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); - b2Vec3 impulse = m_mass.Solve33(-Cdot); + b2Vec3 impulse = -m_mass.Solve33(Cdot); if (m_limitState == e_equalLimits) { @@ -214,7 +226,8 @@ void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step) float32 newImpulse = m_impulse.z + impulse.z; if (newImpulse < 0.0f) { - b2Vec2 reduced = m_mass.Solve22(-Cdot1); + b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y); + b2Vec2 reduced = m_mass.Solve22(rhs); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -m_impulse.z; @@ -222,13 +235,18 @@ void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step) m_impulse.y += reduced.y; m_impulse.z = 0.0f; } + else + { + m_impulse += impulse; + } } else if (m_limitState == e_atUpperLimit) { float32 newImpulse = m_impulse.z + impulse.z; if (newImpulse > 0.0f) { - b2Vec2 reduced = m_mass.Solve22(-Cdot1); + b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y); + b2Vec2 reduced = m_mass.Solve22(rhs); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -m_impulse.z; @@ -236,57 +254,60 @@ void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step) m_impulse.y += reduced.y; m_impulse.z = 0.0f; } + else + { + m_impulse += impulse; + } } b2Vec2 P(impulse.x, impulse.y); - v1 -= m1 * P; - w1 -= i1 * (b2Cross(r1, P) + impulse.z); + vA -= mA * P; + wA -= iA * (b2Cross(m_rA, P) + impulse.z); - v2 += m2 * P; - w2 += i2 * (b2Cross(r2, P) + impulse.z); + vB += mB * P; + wB += iB * (b2Cross(m_rB, P) + impulse.z); } else { - b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); - // Solve point-to-point constraint - b2Vec2 Cdot = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1); + b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); b2Vec2 impulse = m_mass.Solve22(-Cdot); m_impulse.x += impulse.x; m_impulse.y += impulse.y; - v1 -= m1 * impulse; - w1 -= i1 * b2Cross(r1, impulse); + vA -= mA * impulse; + wA -= iA * b2Cross(m_rA, impulse); - v2 += m2 * impulse; - w2 += i2 * b2Cross(r2, impulse); + vB += mB * impulse; + wB += iB * b2Cross(m_rB, impulse); } - b1->m_linearVelocity = v1; - b1->m_angularVelocity = w1; - b2->m_linearVelocity = v2; - b2->m_angularVelocity = w2; + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -bool b2RevoluteJoint::SolvePositionConstraints(float32 baumgarte) +bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data) { - // TODO_ERIN block solve with limit. + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; - B2_NOT_USED(baumgarte); - - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; + b2Rot qA(aA), qB(aB); float32 angularError = 0.0f; float32 positionError = 0.0f; + bool fixedRotation = (m_invIA + m_invIB == 0.0f); + // Solve angular limit constraint. - if (m_enableLimit && m_limitState != e_inactiveLimit) + if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false) { - float32 angle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle; + float32 angle = aB - aA - m_referenceAngle; float32 limitImpulse = 0.0f; if (m_limitState == e_equalLimits) @@ -315,79 +336,54 @@ bool b2RevoluteJoint::SolvePositionConstraints(float32 baumgarte) limitImpulse = -m_motorMass * C; } - b1->m_sweep.a -= b1->m_invI * limitImpulse; - b2->m_sweep.a += b2->m_invI * limitImpulse; - - b1->SynchronizeTransform(); - b2->SynchronizeTransform(); + aA -= m_invIA * limitImpulse; + aB += m_invIB * limitImpulse; } // Solve point-to-point constraint. { - b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); + qA.Set(aA); + qB.Set(aB); + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 C = b2->m_sweep.c + r2 - b1->m_sweep.c - r1; + b2Vec2 C = cB + rB - cA - rA; positionError = C.Length(); - float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass; - float32 invI1 = b1->m_invI, invI2 = b2->m_invI; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; - // Handle large detachment. - const float32 k_allowedStretch = 10.0f * b2_linearSlop; - if (C.LengthSquared() > k_allowedStretch * k_allowedStretch) - { - // Use a particle solution (no rotation). - b2Vec2 u = C; u.Normalize(); - float32 m = invMass1 + invMass2; - if (m > 0.0f) - { - m = 1.0f / m; - } - b2Vec2 impulse = m * (-C); - const float32 k_beta = 0.5f; - b1->m_sweep.c -= k_beta * invMass1 * impulse; - b2->m_sweep.c += k_beta * invMass2 * impulse; + b2Mat22 K; + K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y; + K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y; + K.ey.x = K.ex.y; + K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x; - C = b2->m_sweep.c + r2 - b1->m_sweep.c - r1; - } + b2Vec2 impulse = -K.Solve(C); - b2Mat22 K1; - K1.col1.x = invMass1 + invMass2; K1.col2.x = 0.0f; - K1.col1.y = 0.0f; K1.col2.y = invMass1 + invMass2; + cA -= mA * impulse; + aA -= iA * b2Cross(rA, impulse); - b2Mat22 K2; - K2.col1.x = invI1 * r1.y * r1.y; K2.col2.x = -invI1 * r1.x * r1.y; - K2.col1.y = -invI1 * r1.x * r1.y; K2.col2.y = invI1 * r1.x * r1.x; - - b2Mat22 K3; - K3.col1.x = invI2 * r2.y * r2.y; K3.col2.x = -invI2 * r2.x * r2.y; - K3.col1.y = -invI2 * r2.x * r2.y; K3.col2.y = invI2 * r2.x * r2.x; - - b2Mat22 K = K1 + K2 + K3; - b2Vec2 impulse = K.Solve(-C); - - b1->m_sweep.c -= b1->m_invMass * impulse; - b1->m_sweep.a -= b1->m_invI * b2Cross(r1, impulse); - - b2->m_sweep.c += b2->m_invMass * impulse; - b2->m_sweep.a += b2->m_invI * b2Cross(r2, impulse); - - b1->SynchronizeTransform(); - b2->SynchronizeTransform(); + cB += mB * impulse; + aB += iB * b2Cross(rB, impulse); } + + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; return positionError <= b2_linearSlop && angularError <= b2_angularSlop; } b2Vec2 b2RevoluteJoint::GetAnchorA() const { - return m_bodyA->GetWorldPoint(m_localAnchor1); + return m_bodyA->GetWorldPoint(m_localAnchorA); } b2Vec2 b2RevoluteJoint::GetAnchorB() const { - return m_bodyB->GetWorldPoint(m_localAnchor2); + return m_bodyB->GetWorldPoint(m_localAnchorB); } b2Vec2 b2RevoluteJoint::GetReactionForce(float32 inv_dt) const @@ -403,16 +399,16 @@ float32 b2RevoluteJoint::GetReactionTorque(float32 inv_dt) const float32 b2RevoluteJoint::GetJointAngle() const { - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; - return b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle; + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; + return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle; } float32 b2RevoluteJoint::GetJointSpeed() const { - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; - return b2->m_angularVelocity - b1->m_angularVelocity; + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; + return bB->m_angularVelocity - bA->m_angularVelocity; } bool b2RevoluteJoint::IsMotorEnabled() const @@ -427,9 +423,9 @@ void b2RevoluteJoint::EnableMotor(bool flag) m_enableMotor = flag; } -float32 b2RevoluteJoint::GetMotorTorque() const +float32 b2RevoluteJoint::GetMotorTorque(float32 inv_dt) const { - return m_motorImpulse; + return inv_dt * m_motorImpulse; } void b2RevoluteJoint::SetMotorSpeed(float32 speed) @@ -453,9 +449,13 @@ bool b2RevoluteJoint::IsLimitEnabled() const void b2RevoluteJoint::EnableLimit(bool flag) { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableLimit = flag; + if (flag != m_enableLimit) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableLimit = flag; + m_impulse.z = 0.0f; + } } float32 b2RevoluteJoint::GetLowerLimit() const @@ -471,8 +471,13 @@ float32 b2RevoluteJoint::GetUpperLimit() const void b2RevoluteJoint::SetLimits(float32 lower, float32 upper) { b2Assert(lower <= upper); - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_lowerAngle = lower; - m_upperAngle = upper; + + if (lower != m_lowerAngle || upper != m_upperAngle) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_impulse.z = 0.0f; + m_lowerAngle = lower; + m_upperAngle = upper; + } } diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.h old mode 100644 new mode 100755 index c0180beba..03eae0b3a --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -93,9 +93,6 @@ public: b2Vec2 GetAnchorA() const; b2Vec2 GetAnchorB() const; - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - /// Get the current joint angle in radians. float32 GetJointAngle() const; @@ -132,8 +129,17 @@ public: /// Set the maximum motor torque, usually in N-m. void SetMaxMotorTorque(float32 torque); - /// Get the current motor torque, usually in N-m. - float32 GetMotorTorque() const; + /// Get the reaction force given the inverse time step. + /// Unit is N. + b2Vec2 GetReactionForce(float32 inv_dt) const; + + /// Get the reaction torque due to the joint limit given the inverse time step. + /// Unit is N*m. + float32 GetReactionTorque(float32 inv_dt) const; + + /// Get the current motor torque given the inverse time step. + /// Unit is N*m. + float32 GetMotorTorque(float32 inv_dt) const; protected: @@ -142,19 +148,16 @@ protected: b2RevoluteJoint(const b2RevoluteJointDef* def); - void InitVelocityConstraints(const b2TimeStep& step); - void SolveVelocityConstraints(const b2TimeStep& step); + void InitVelocityConstraints(const b2SolverData& data); + void SolveVelocityConstraints(const b2SolverData& data); + bool SolvePositionConstraints(const b2SolverData& data); - bool SolvePositionConstraints(float32 baumgarte); - - b2Vec2 m_localAnchor1; // relative - b2Vec2 m_localAnchor2; + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; b2Vec3 m_impulse; float32 m_motorImpulse; - b2Mat33 m_mass; // effective mass for point-to-point constraint. - float32 m_motorMass; // effective mass for motor/limit angular constraint. - bool m_enableMotor; float32 m_maxMotorTorque; float32 m_motorSpeed; @@ -163,6 +166,20 @@ protected: float32 m_referenceAngle; float32 m_lowerAngle; float32 m_upperAngle; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + b2Mat33 m_mass; // effective mass for point-to-point constraint. + float32 m_motorMass; // effective mass for motor/limit angular constraint. b2LimitState m_limitState; }; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RopeJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RopeJoint.cpp new file mode 100755 index 000000000..4b1e666c1 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RopeJoint.cpp @@ -0,0 +1,226 @@ +/* +* Copyright (c) 2007-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include + + +// Limit: +// C = norm(pB - pA) - L +// u = (pB - pA) / norm(pB - pA) +// Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA)) +// J = [-u -cross(rA, u) u cross(rB, u)] +// K = J * invM * JT +// = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2 + +b2RopeJoint::b2RopeJoint(const b2RopeJointDef* def) +: b2Joint(def) +{ + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; + + m_maxLength = def->maxLength; + + m_mass = 0.0f; + m_impulse = 0.0f; + m_state = e_inactiveLimit; + m_length = 0.0f; +} + +void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data) +{ + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; + + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + m_u = cB + m_rB - cA - m_rA; + + m_length = m_u.Length(); + + float32 C = m_length - m_maxLength; + if (C > 0.0f) + { + m_state = e_atUpperLimit; + } + else + { + m_state = e_inactiveLimit; + } + + if (m_length > b2_linearSlop) + { + m_u *= 1.0f / m_length; + } + else + { + m_u.SetZero(); + m_mass = 0.0f; + m_impulse = 0.0f; + return; + } + + // Compute effective mass. + float32 crA = b2Cross(m_rA, m_u); + float32 crB = b2Cross(m_rB, m_u); + float32 invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB; + + m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + + if (data.step.warmStarting) + { + // Scale the impulse to support a variable time step. + m_impulse *= data.step.dtRatio; + + b2Vec2 P = m_impulse * m_u; + vA -= m_invMassA * P; + wA -= m_invIA * b2Cross(m_rA, P); + vB += m_invMassB * P; + wB += m_invIB * b2Cross(m_rB, P); + } + else + { + m_impulse = 0.0f; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; +} + +void b2RopeJoint::SolveVelocityConstraints(const b2SolverData& data) +{ + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + // Cdot = dot(u, v + cross(w, r)) + b2Vec2 vpA = vA + b2Cross(wA, m_rA); + b2Vec2 vpB = vB + b2Cross(wB, m_rB); + float32 C = m_length - m_maxLength; + float32 Cdot = b2Dot(m_u, vpB - vpA); + + // Predictive constraint. + if (C < 0.0f) + { + Cdot += data.step.inv_dt * C; + } + + float32 impulse = -m_mass * Cdot; + float32 oldImpulse = m_impulse; + m_impulse = b2Min(0.0f, m_impulse + impulse); + impulse = m_impulse - oldImpulse; + + b2Vec2 P = impulse * m_u; + vA -= m_invMassA * P; + wA -= m_invIA * b2Cross(m_rA, P); + vB += m_invMassB * P; + wB += m_invIB * b2Cross(m_rB, P); + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; +} + +bool b2RopeJoint::SolvePositionConstraints(const b2SolverData& data) +{ + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + + b2Rot qA(aA), qB(aB); + + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + b2Vec2 u = cB + rB - cA - rA; + + float32 length = u.Normalize(); + float32 C = length - m_maxLength; + + C = b2Clamp(C, 0.0f, b2_maxLinearCorrection); + + float32 impulse = -m_mass * C; + b2Vec2 P = impulse * u; + + cA -= m_invMassA * P; + aA -= m_invIA * b2Cross(rA, P); + cB += m_invMassB * P; + aB += m_invIB * b2Cross(rB, P); + + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; + + return length - m_maxLength < b2_linearSlop; +} + +b2Vec2 b2RopeJoint::GetAnchorA() const +{ + return m_bodyA->GetWorldPoint(m_localAnchorA); +} + +b2Vec2 b2RopeJoint::GetAnchorB() const +{ + return m_bodyB->GetWorldPoint(m_localAnchorB); +} + +b2Vec2 b2RopeJoint::GetReactionForce(float32 inv_dt) const +{ + b2Vec2 F = (inv_dt * m_impulse) * m_u; + return F; +} + +float32 b2RopeJoint::GetReactionTorque(float32 inv_dt) const +{ + B2_NOT_USED(inv_dt); + return 0.0f; +} + +float32 b2RopeJoint::GetMaxLength() const +{ + return m_maxLength; +} + +b2LimitState b2RopeJoint::GetLimitState() const +{ + return m_state; +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RopeJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RopeJoint.h new file mode 100755 index 000000000..660b4ed50 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RopeJoint.h @@ -0,0 +1,104 @@ +/* +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B2_ROPE_JOINT_H +#define B2_ROPE_JOINT_H + +#include + +/// Rope joint definition. This requires two body anchor points and +/// a maximum lengths. +/// Note: by default the connected objects will not collide. +/// see collideConnected in b2JointDef. +struct b2RopeJointDef : public b2JointDef +{ + b2RopeJointDef() + { + type = e_ropeJoint; + localAnchorA.Set(-1.0f, 0.0f); + localAnchorB.Set(1.0f, 0.0f); + maxLength = 0.0f; + } + + /// The local anchor point relative to bodyA's origin. + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin. + b2Vec2 localAnchorB; + + /// The maximum length of the rope. + /// Warning: this must be larger than b2_linearSlop or + /// the joint will have no effect. + float32 maxLength; +}; + +/// A rope joint enforces a maximum distance between two points +/// on two bodies. It has no other effect. +/// Warning: if you attempt to change the maximum length during +/// the simulation you will get some non-physical behavior. +/// A model that would allow you to dynamically modify the length +/// would have some sponginess, so I chose not to implement it +/// that way. See b2DistanceJoint if you want to dynamically +/// control length. +class b2RopeJoint : public b2Joint +{ +public: + b2Vec2 GetAnchorA() const; + b2Vec2 GetAnchorB() const; + + b2Vec2 GetReactionForce(float32 inv_dt) const; + float32 GetReactionTorque(float32 inv_dt) const; + + /// Get the maximum length of the rope. + float32 GetMaxLength() const; + + b2LimitState GetLimitState() const; + +protected: + + friend class b2Joint; + b2RopeJoint(const b2RopeJointDef* data); + + void InitVelocityConstraints(const b2SolverData& data); + void SolveVelocityConstraints(const b2SolverData& data); + bool SolvePositionConstraints(const b2SolverData& data); + + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + float32 m_maxLength; + float32 m_length; + float32 m_impulse; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_u; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + float32 m_mass; + b2LimitState m_state; +}; + +#endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.cpp old mode 100644 new mode 100755 index 49b5513ba..11eb5ec65 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -53,14 +53,31 @@ b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def) m_impulse.SetZero(); } -void b2WeldJoint::InitVelocityConstraints(const b2TimeStep& step) +void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data) { - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; - // Compute the effective mass matrix. - b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter()); - b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter()); + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] @@ -71,128 +88,121 @@ void b2WeldJoint::InitVelocityConstraints(const b2TimeStep& step) // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - float32 mA = bA->m_invMass, mB = bB->m_invMass; - float32 iA = bA->m_invI, iB = bB->m_invI; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; - m_mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; - m_mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; - m_mass.col3.x = -rA.y * iA - rB.y * iB; - m_mass.col1.y = m_mass.col2.x; - m_mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; - m_mass.col3.y = rA.x * iA + rB.x * iB; - m_mass.col1.z = m_mass.col3.x; - m_mass.col2.z = m_mass.col3.y; - m_mass.col3.z = iA + iB; + m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; + m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; + m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB; + m_mass.ex.y = m_mass.ey.x; + m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; + m_mass.ez.y = m_rA.x * iA + m_rB.x * iB; + m_mass.ex.z = m_mass.ez.x; + m_mass.ey.z = m_mass.ez.y; + m_mass.ez.z = iA + iB; - if (step.warmStarting) + if (data.step.warmStarting) { // Scale impulses to support a variable time step. - m_impulse *= step.dtRatio; + m_impulse *= data.step.dtRatio; b2Vec2 P(m_impulse.x, m_impulse.y); - bA->m_linearVelocity -= mA * P; - bA->m_angularVelocity -= iA * (b2Cross(rA, P) + m_impulse.z); + vA -= mA * P; + wA -= iA * (b2Cross(m_rA, P) + m_impulse.z); - bB->m_linearVelocity += mB * P; - bB->m_angularVelocity += iB * (b2Cross(rB, P) + m_impulse.z); + vB += mB * P; + wB += iB * (b2Cross(m_rB, P) + m_impulse.z); } else { m_impulse.SetZero(); } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2WeldJoint::SolveVelocityConstraints(const b2TimeStep& step) +void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data) { - B2_NOT_USED(step); + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; - b2Vec2 vA = bA->m_linearVelocity; - float32 wA = bA->m_angularVelocity; - b2Vec2 vB = bB->m_linearVelocity; - float32 wB = bB->m_angularVelocity; - - float32 mA = bA->m_invMass, mB = bB->m_invMass; - float32 iA = bA->m_invI, iB = bB->m_invI; - - b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter()); - b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter()); - - // Solve point-to-point constraint - b2Vec2 Cdot1 = vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA); + b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); float32 Cdot2 = wB - wA; b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); - b2Vec3 impulse = m_mass.Solve33(-Cdot); + b2Vec3 impulse = -m_mass.Solve33(Cdot); m_impulse += impulse; b2Vec2 P(impulse.x, impulse.y); vA -= mA * P; - wA -= iA * (b2Cross(rA, P) + impulse.z); + wA -= iA * (b2Cross(m_rA, P) + impulse.z); vB += mB * P; - wB += iB * (b2Cross(rB, P) + impulse.z); + wB += iB * (b2Cross(m_rB, P) + impulse.z); - bA->m_linearVelocity = vA; - bA->m_angularVelocity = wA; - bB->m_linearVelocity = vB; - bB->m_angularVelocity = wB; + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -bool b2WeldJoint::SolvePositionConstraints(float32 baumgarte) +bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data) { - B2_NOT_USED(baumgarte); + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; + b2Rot qA(aA), qB(aB); - float32 mA = bA->m_invMass, mB = bB->m_invMass; - float32 iA = bA->m_invI, iB = bB->m_invI; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; - b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter()); - b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter()); + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 C1 = bB->m_sweep.c + rB - bA->m_sweep.c - rA; - float32 C2 = bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle; + b2Vec2 C1 = cB + rB - cA - rA; + float32 C2 = aB - aA - m_referenceAngle; - // Handle large detachment. - const float32 k_allowedStretch = 10.0f * b2_linearSlop; float32 positionError = C1.Length(); float32 angularError = b2Abs(C2); - if (positionError > k_allowedStretch) - { - iA *= 1.0f; - iB *= 1.0f; - } - m_mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; - m_mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; - m_mass.col3.x = -rA.y * iA - rB.y * iB; - m_mass.col1.y = m_mass.col2.x; - m_mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; - m_mass.col3.y = rA.x * iA + rB.x * iB; - m_mass.col1.z = m_mass.col3.x; - m_mass.col2.z = m_mass.col3.y; - m_mass.col3.z = iA + iB; + m_mass.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; + m_mass.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; + m_mass.ez.x = -rA.y * iA - rB.y * iB; + m_mass.ex.y = m_mass.ey.x; + m_mass.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; + m_mass.ez.y = rA.x * iA + rB.x * iB; + m_mass.ex.z = m_mass.ez.x; + m_mass.ey.z = m_mass.ez.y; + m_mass.ez.z = iA + iB; b2Vec3 C(C1.x, C1.y, C2); - b2Vec3 impulse = m_mass.Solve33(-C); + b2Vec3 impulse = -m_mass.Solve33(C); b2Vec2 P(impulse.x, impulse.y); - bA->m_sweep.c -= mA * P; - bA->m_sweep.a -= iA * (b2Cross(rA, P) + impulse.z); + cA -= mA * P; + aA -= iA * (b2Cross(rA, P) + impulse.z); - bB->m_sweep.c += mB * P; - bB->m_sweep.a += iB * (b2Cross(rB, P) + impulse.z); + cB += mB * P; + aB += iB * (b2Cross(rB, P) + impulse.z); - bA->SynchronizeTransform(); - bB->SynchronizeTransform(); + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; return positionError <= b2_linearSlop && angularError <= b2_angularSlop; } diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.h old mode 100644 new mode 100755 index 4e63b6aa6..51c4b402b --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -65,17 +65,27 @@ protected: b2WeldJoint(const b2WeldJointDef* def); - void InitVelocityConstraints(const b2TimeStep& step); - void SolveVelocityConstraints(const b2TimeStep& step); - - bool SolvePositionConstraints(float32 baumgarte); + void InitVelocityConstraints(const b2SolverData& data); + void SolveVelocityConstraints(const b2SolverData& data); + bool SolvePositionConstraints(const b2SolverData& data); + // Solver shared b2Vec2 m_localAnchorA; b2Vec2 m_localAnchorB; float32 m_referenceAngle; - b2Vec3 m_impulse; + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; b2Mat33 m_mass; }; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WheelJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WheelJoint.cpp new file mode 100755 index 000000000..826192347 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WheelJoint.cpp @@ -0,0 +1,404 @@ +/* +* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include + +// Linear constraint (point-to-line) +// d = pB - pA = xB + rB - xA - rA +// C = dot(ay, d) +// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA)) +// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB) +// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)] + +// Spring linear constraint +// C = dot(ax, d) +// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB) +// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)] + +// Motor rotational constraint +// Cdot = wB - wA +// J = [0 0 -1 0 0 1] + +void b2WheelJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis) +{ + bodyA = bA; + bodyB = bB; + localAnchorA = bodyA->GetLocalPoint(anchor); + localAnchorB = bodyB->GetLocalPoint(anchor); + localAxisA = bodyA->GetLocalVector(axis); +} + +b2WheelJoint::b2WheelJoint(const b2WheelJointDef* def) +: b2Joint(def) +{ + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; + m_localXAxisA = def->localAxisA; + m_localYAxisA = b2Cross(1.0f, m_localXAxisA); + + m_mass = 0.0f; + m_impulse = 0.0f; + m_motorMass = 0.0; + m_motorImpulse = 0.0f; + m_springMass = 0.0f; + m_springImpulse = 0.0f; + + m_maxMotorTorque = def->maxMotorTorque; + m_motorSpeed = def->motorSpeed; + m_enableMotor = def->enableMotor; + + m_frequencyHz = def->frequencyHz; + m_dampingRatio = def->dampingRatio; + + m_bias = 0.0f; + m_gamma = 0.0f; + + m_ax.SetZero(); + m_ay.SetZero(); +} + +void b2WheelJoint::InitVelocityConstraints(const b2SolverData& data) +{ + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + // Compute the effective masses. + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + b2Vec2 d = cB + rB - cA - rA; + + // Point to line constraint + { + m_ay = b2Mul(qA, m_localYAxisA); + m_sAy = b2Cross(d + rA, m_ay); + m_sBy = b2Cross(rB, m_ay); + + m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy; + + if (m_mass > 0.0f) + { + m_mass = 1.0f / m_mass; + } + } + + // Spring constraint + m_springMass = 0.0f; + m_bias = 0.0f; + m_gamma = 0.0f; + if (m_frequencyHz > 0.0f) + { + m_ax = b2Mul(qA, m_localXAxisA); + m_sAx = b2Cross(d + rA, m_ax); + m_sBx = b2Cross(rB, m_ax); + + float32 invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx; + + if (invMass > 0.0f) + { + m_springMass = 1.0f / invMass; + + float32 C = b2Dot(d, m_ax); + + // Frequency + float32 omega = 2.0f * b2_pi * m_frequencyHz; + + // Damping coefficient + float32 d = 2.0f * m_springMass * m_dampingRatio * omega; + + // Spring stiffness + float32 k = m_springMass * omega * omega; + + // magic formulas + float32 h = data.step.dt; + m_gamma = h * (d + h * k); + if (m_gamma > 0.0f) + { + m_gamma = 1.0f / m_gamma; + } + + m_bias = C * h * k * m_gamma; + + m_springMass = invMass + m_gamma; + if (m_springMass > 0.0f) + { + m_springMass = 1.0f / m_springMass; + } + } + } + else + { + m_springImpulse = 0.0f; + } + + // Rotational motor + if (m_enableMotor) + { + m_motorMass = iA + iB; + if (m_motorMass > 0.0f) + { + m_motorMass = 1.0f / m_motorMass; + } + } + else + { + m_motorMass = 0.0f; + m_motorImpulse = 0.0f; + } + + if (data.step.warmStarting) + { + // Account for variable time step. + m_impulse *= data.step.dtRatio; + m_springImpulse *= data.step.dtRatio; + m_motorImpulse *= data.step.dtRatio; + + b2Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax; + float32 LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse; + float32 LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse; + + vA -= m_invMassA * P; + wA -= m_invIA * LA; + + vB += m_invMassB * P; + wB += m_invIB * LB; + } + else + { + m_impulse = 0.0f; + m_springImpulse = 0.0f; + m_motorImpulse = 0.0f; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; +} + +void b2WheelJoint::SolveVelocityConstraints(const b2SolverData& data) +{ + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + // Solve spring constraint + { + float32 Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA; + float32 impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse); + m_springImpulse += impulse; + + b2Vec2 P = impulse * m_ax; + float32 LA = impulse * m_sAx; + float32 LB = impulse * m_sBx; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + // Solve rotational motor constraint + { + float32 Cdot = wB - wA - m_motorSpeed; + float32 impulse = -m_motorMass * Cdot; + + float32 oldImpulse = m_motorImpulse; + float32 maxImpulse = data.step.dt * m_maxMotorTorque; + m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = m_motorImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // Solve point to line constraint + { + float32 Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA; + float32 impulse = -m_mass * Cdot; + m_impulse += impulse; + + b2Vec2 P = impulse * m_ay; + float32 LA = impulse * m_sAy; + float32 LB = impulse * m_sBy; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; +} + +bool b2WheelJoint::SolvePositionConstraints(const b2SolverData& data) +{ + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + + b2Rot qA(aA), qB(aB); + + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + b2Vec2 d = (cB - cA) + rB - rA; + + b2Vec2 ay = b2Mul(qA, m_localYAxisA); + + float32 sAy = b2Cross(d + rA, ay); + float32 sBy = b2Cross(rB, ay); + + float32 C = b2Dot(d, ay); + + float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy; + + float32 impulse; + if (k != 0.0f) + { + impulse = - C / k; + } + else + { + impulse = 0.0f; + } + + b2Vec2 P = impulse * ay; + float32 LA = impulse * sAy; + float32 LB = impulse * sBy; + + cA -= m_invMassA * P; + aA -= m_invIA * LA; + cB += m_invMassB * P; + aB += m_invIB * LB; + + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; + + return b2Abs(C) <= b2_linearSlop; +} + +b2Vec2 b2WheelJoint::GetAnchorA() const +{ + return m_bodyA->GetWorldPoint(m_localAnchorA); +} + +b2Vec2 b2WheelJoint::GetAnchorB() const +{ + return m_bodyB->GetWorldPoint(m_localAnchorB); +} + +b2Vec2 b2WheelJoint::GetReactionForce(float32 inv_dt) const +{ + return inv_dt * (m_impulse * m_ay + m_springImpulse * m_ax); +} + +float32 b2WheelJoint::GetReactionTorque(float32 inv_dt) const +{ + return inv_dt * m_motorImpulse; +} + +float32 b2WheelJoint::GetJointTranslation() const +{ + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; + + b2Vec2 pA = bA->GetWorldPoint(m_localAnchorA); + b2Vec2 pB = bB->GetWorldPoint(m_localAnchorB); + b2Vec2 d = pB - pA; + b2Vec2 axis = bA->GetWorldVector(m_localXAxisA); + + float32 translation = b2Dot(d, axis); + return translation; +} + +float32 b2WheelJoint::GetJointSpeed() const +{ + float32 wA = m_bodyA->m_angularVelocity; + float32 wB = m_bodyB->m_angularVelocity; + return wB - wA; +} + +bool b2WheelJoint::IsMotorEnabled() const +{ + return m_enableMotor; +} + +void b2WheelJoint::EnableMotor(bool flag) +{ + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableMotor = flag; +} + +void b2WheelJoint::SetMotorSpeed(float32 speed) +{ + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_motorSpeed = speed; +} + +void b2WheelJoint::SetMaxMotorTorque(float32 torque) +{ + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_maxMotorTorque = torque; +} + +float32 b2WheelJoint::GetMotorTorque(float32 inv_dt) const +{ + return inv_dt * m_motorImpulse; +} + + + + + diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WheelJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WheelJoint.h new file mode 100755 index 000000000..7398d218c --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WheelJoint.h @@ -0,0 +1,199 @@ +/* +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B2_WHEEL_JOINT_H +#define B2_WHEEL_JOINT_H + +#include + +/// Wheel joint definition. This requires defining a line of +/// motion using an axis and an anchor point. The definition uses local +/// anchor points and a local axis so that the initial configuration +/// can violate the constraint slightly. The joint translation is zero +/// when the local anchor points coincide in world space. Using local +/// anchors and a local axis helps when saving and loading a game. +struct b2WheelJointDef : public b2JointDef +{ + b2WheelJointDef() + { + type = e_wheelJoint; + localAnchorA.SetZero(); + localAnchorB.SetZero(); + localAxisA.Set(1.0f, 0.0f); + enableMotor = false; + maxMotorTorque = 0.0f; + motorSpeed = 0.0f; + frequencyHz = 2.0f; + dampingRatio = 0.7f; + } + + /// Initialize the bodies, anchors, axis, and reference angle using the world + /// anchor and world axis. + void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis); + + /// The local anchor point relative to body1's origin. + b2Vec2 localAnchorA; + + /// The local anchor point relative to body2's origin. + b2Vec2 localAnchorB; + + /// The local translation axis in body1. + b2Vec2 localAxisA; + + /// Enable/disable the joint motor. + bool enableMotor; + + /// The maximum motor torque, usually in N-m. + float32 maxMotorTorque; + + /// The desired motor speed in radians per second. + float32 motorSpeed; + + /// Suspension frequency, zero indicates no suspension + float32 frequencyHz; + + /// Suspension damping ratio, one indicates critical damping + float32 dampingRatio; +}; + +/// A wheel joint. This joint provides two degrees of freedom: translation +/// along an axis fixed in body1 and rotation in the plane. You can use a +/// joint limit to restrict the range of motion and a joint motor to drive +/// the rotation or to model rotational friction. +/// This joint is designed for vehicle suspensions. +class b2WheelJoint : public b2Joint +{ +public: + b2Vec2 GetAnchorA() const; + b2Vec2 GetAnchorB() const; + + b2Vec2 GetReactionForce(float32 inv_dt) const; + float32 GetReactionTorque(float32 inv_dt) const; + + /// Get the current joint translation, usually in meters. + float32 GetJointTranslation() const; + + /// Get the current joint translation speed, usually in meters per second. + float32 GetJointSpeed() const; + + /// Is the joint motor enabled? + bool IsMotorEnabled() const; + + /// Enable/disable the joint motor. + void EnableMotor(bool flag); + + /// Set the motor speed, usually in radians per second. + void SetMotorSpeed(float32 speed); + + /// Get the motor speed, usually in radians per second. + float32 GetMotorSpeed() const; + + /// Set/Get the maximum motor force, usually in N-m. + void SetMaxMotorTorque(float32 torque); + float32 GetMaxMotorTorque() const; + + /// Get the current motor torque given the inverse time step, usually in N-m. + float32 GetMotorTorque(float32 inv_dt) const; + + /// Set/Get the spring frequency in hertz. Setting the frequency to zero disables the spring. + void SetSpringFrequencyHz(float32 hz); + float32 GetSpringFrequencyHz() const; + + /// Set/Get the spring damping ratio + void SetSpringDampingRatio(float32 ratio); + float32 GetSpringDampingRatio() const; + +protected: + + friend class b2Joint; + b2WheelJoint(const b2WheelJointDef* def); + + void InitVelocityConstraints(const b2SolverData& data); + void SolveVelocityConstraints(const b2SolverData& data); + bool SolvePositionConstraints(const b2SolverData& data); + + float32 m_frequencyHz; + float32 m_dampingRatio; + + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + b2Vec2 m_localXAxisA; + b2Vec2 m_localYAxisA; + + float32 m_impulse; + float32 m_motorImpulse; + float32 m_springImpulse; + + float32 m_maxMotorTorque; + float32 m_motorSpeed; + bool m_enableMotor; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + + b2Vec2 m_ax, m_ay; + float32 m_sAx, m_sBx; + float32 m_sAy, m_sBy; + + float32 m_mass; + float32 m_motorMass; + float32 m_springMass; + + float32 m_bias; + float32 m_gamma; +}; + +inline float32 b2WheelJoint::GetMotorSpeed() const +{ + return m_motorSpeed; +} + +inline float32 b2WheelJoint::GetMaxMotorTorque() const +{ + return m_maxMotorTorque; +} + +inline void b2WheelJoint::SetSpringFrequencyHz(float32 hz) +{ + m_frequencyHz = hz; +} + +inline float32 b2WheelJoint::GetSpringFrequencyHz() const +{ + return m_frequencyHz; +} + +inline void b2WheelJoint::SetSpringDampingRatio(float32 ratio) +{ + m_dampingRatio = ratio; +} + +inline float32 b2WheelJoint::GetSpringDampingRatio() const +{ + return m_dampingRatio; +} + +#endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2Body.cpp b/src/modules/physics/box2d/Box2D/Dynamics/b2Body.cpp old mode 100644 new mode 100755 index 4b8865148..858977c81 --- a/src/modules/physics/box2d/Box2D/Dynamics/b2Body.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2Body.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -28,7 +28,6 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world) b2Assert(bd->linearVelocity.IsValid()); b2Assert(b2IsValid(bd->angle)); b2Assert(b2IsValid(bd->angularVelocity)); - b2Assert(b2IsValid(bd->inertiaScale) && bd->inertiaScale >= 0.0f); b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f); b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f); @@ -57,12 +56,15 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world) m_world = world; - m_xf.position = bd->position; - m_xf.R.Set(bd->angle); + m_xf.p = bd->position; + m_xf.q.Set(bd->angle); m_sweep.localCenter.SetZero(); - m_sweep.a0 = m_sweep.a = bd->angle; - m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); + m_sweep.c0 = m_xf.p; + m_sweep.c = m_xf.p; + m_sweep.a0 = bd->angle; + m_sweep.a = bd->angle; + m_sweep.alpha0 = 0.0f; m_jointList = NULL; m_contactList = NULL; @@ -74,6 +76,7 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world) m_linearDamping = bd->linearDamping; m_angularDamping = bd->angularDamping; + m_gravityScale = bd->gravityScale; m_force.SetZero(); m_torque = 0.0f; @@ -109,6 +112,12 @@ b2Body::~b2Body() void b2Body::SetType(b2BodyType type) { + b2Assert(m_world->IsLocked() == false); + if (m_world->IsLocked() == true) + { + return; + } + if (m_type == type) { return; @@ -122,6 +131,9 @@ void b2Body::SetType(b2BodyType type) { m_linearVelocity.SetZero(); m_angularVelocity = 0.0f; + m_sweep.a0 = m_sweep.a; + m_sweep.c0 = m_sweep.c; + SynchronizeFixtures(); } SetAwake(true); @@ -130,9 +142,9 @@ void b2Body::SetType(b2BodyType type) m_torque = 0.0f; // Since the body type changed, we need to flag contacts for filtering. - for (b2ContactEdge* ce = m_contactList; ce; ce = ce->next) + for (b2Fixture* f = m_fixtureList; f; f = f->m_next) { - ce->contact->FlagForFiltering(); + f->Refilter(); } } @@ -153,7 +165,7 @@ b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def) if (m_flags & e_activeFlag) { b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - fixture->CreateProxy(broadPhase, m_xf); + fixture->CreateProxies(broadPhase, m_xf); } fixture->m_next = m_fixtureList; @@ -235,13 +247,8 @@ void b2Body::DestroyFixture(b2Fixture* fixture) if (m_flags & e_activeFlag) { - b2Assert(fixture->m_proxyId != b2BroadPhase::e_nullProxy); b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - fixture->DestroyProxy(broadPhase); - } - else - { - b2Assert(fixture->m_proxyId == b2BroadPhase::e_nullProxy); + fixture->DestroyProxies(broadPhase); } fixture->Destroy(allocator); @@ -268,14 +275,16 @@ void b2Body::ResetMassData() // Static and kinematic bodies have zero mass. if (m_type == b2_staticBody || m_type == b2_kinematicBody) { - m_sweep.c0 = m_sweep.c = m_xf.position; + m_sweep.c0 = m_xf.p; + m_sweep.c = m_xf.p; + m_sweep.a0 = m_sweep.a; return; } b2Assert(m_type == b2_dynamicBody); // Accumulate mass over all fixtures. - b2Vec2 center = b2Vec2_zero; + b2Vec2 localCenter = b2Vec2_zero; for (b2Fixture* f = m_fixtureList; f; f = f->m_next) { if (f->m_density == 0.0f) @@ -286,7 +295,7 @@ void b2Body::ResetMassData() b2MassData massData; f->GetMassData(&massData); m_mass += massData.mass; - center += massData.mass * massData.center; + localCenter += massData.mass * massData.center; m_I += massData.I; } @@ -294,7 +303,7 @@ void b2Body::ResetMassData() if (m_mass > 0.0f) { m_invMass = 1.0f / m_mass; - center *= m_invMass; + localCenter *= m_invMass; } else { @@ -306,7 +315,7 @@ void b2Body::ResetMassData() if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0) { // Center the inertia about the center of mass. - m_I -= m_mass * b2Dot(center, center); + m_I -= m_mass * b2Dot(localCenter, localCenter); b2Assert(m_I > 0.0f); m_invI = 1.0f / m_I; @@ -319,7 +328,7 @@ void b2Body::ResetMassData() // Move center of mass. b2Vec2 oldCenter = m_sweep.c; - m_sweep.localCenter = center; + m_sweep.localCenter = localCenter; m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); // Update center of mass velocity. @@ -360,7 +369,7 @@ void b2Body::SetMassData(const b2MassData* massData) // Move center of mass. b2Vec2 oldCenter = m_sweep.c; - m_sweep.localCenter = massData->center; + m_sweep.localCenter = massData->center; m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); // Update center of mass velocity. @@ -398,11 +407,14 @@ void b2Body::SetTransform(const b2Vec2& position, float32 angle) return; } - m_xf.R.Set(angle); - m_xf.position = position; + m_xf.q.Set(angle); + m_xf.p = position; - m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); - m_sweep.a0 = m_sweep.a = angle; + m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); + m_sweep.a = angle; + + m_sweep.c0 = m_sweep.c; + m_sweep.a0 = angle; b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; for (b2Fixture* f = m_fixtureList; f; f = f->m_next) @@ -416,8 +428,8 @@ void b2Body::SetTransform(const b2Vec2& position, float32 angle) void b2Body::SynchronizeFixtures() { b2Transform xf1; - xf1.R.Set(m_sweep.a0); - xf1.position = m_sweep.c0 - b2Mul(xf1.R, m_sweep.localCenter); + xf1.q.Set(m_sweep.a0); + xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter); b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; for (b2Fixture* f = m_fixtureList; f; f = f->m_next) @@ -428,6 +440,8 @@ void b2Body::SynchronizeFixtures() void b2Body::SetActive(bool flag) { + b2Assert(m_world->IsLocked() == false); + if (flag == IsActive()) { return; @@ -441,7 +455,7 @@ void b2Body::SetActive(bool flag) b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; for (b2Fixture* f = m_fixtureList; f; f = f->m_next) { - f->CreateProxy(broadPhase, m_xf); + f->CreateProxies(broadPhase, m_xf); } // Contacts are created the next time step. @@ -454,7 +468,7 @@ void b2Body::SetActive(bool flag) b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; for (b2Fixture* f = m_fixtureList; f; f = f->m_next) { - f->DestroyProxy(broadPhase); + f->DestroyProxies(broadPhase); } // Destroy the attached contacts. diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2Body.h b/src/modules/physics/box2d/Box2D/Dynamics/b2Body.h old mode 100644 new mode 100755 index f2f915faa..5ebc5ddd1 --- a/src/modules/physics/box2d/Box2D/Dynamics/b2Body.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2Body.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -40,7 +40,10 @@ enum b2BodyType { b2_staticBody = 0, b2_kinematicBody, - b2_dynamicBody, + b2_dynamicBody + + // TODO_ERIN + //b2_bulletBody, }; /// A body definition holds all the data needed to construct a rigid body. @@ -63,7 +66,7 @@ struct b2BodyDef bullet = false; type = b2_staticBody; active = true; - inertiaScale = 1.0f; + gravityScale = 1.0f; } /// The body type: static, kinematic, or dynamic. @@ -115,8 +118,8 @@ struct b2BodyDef /// Use this to store application specific body data. void* userData; - /// Experimental: scales the inertia tensor. - float32 inertiaScale; + /// Scale the gravity applied to this body. + float32 gravityScale; }; /// A rigid body. These are created via b2World::CreateBody. @@ -198,6 +201,10 @@ public: /// @param point the world position of the point of application. void ApplyForce(const b2Vec2& force, const b2Vec2& point); + /// Apply a force to the center of mass. This wakes up the body. + /// @param force the world force vector, usually in Newtons (N). + void ApplyForceToCenter(const b2Vec2& force); + /// Apply a torque. This affects the angular velocity /// without affecting the linear velocity of the center of mass. /// This wakes up the body. @@ -281,6 +288,12 @@ public: /// Set the angular damping of the body. void SetAngularDamping(float32 angularDamping); + /// Get the gravity scale of the body. + float32 GetGravityScale() const; + + /// Set the gravity scale of the body. + void SetGravityScale(float32 scale); + /// Set the type of this body. This may alter the mass and velocity. void SetType(b2BodyType type); @@ -368,17 +381,18 @@ private: friend class b2Island; friend class b2ContactManager; friend class b2ContactSolver; - friend class b2TOISolver; + friend class b2Contact; friend class b2DistanceJoint; friend class b2GearJoint; - friend class b2LineJoint; + friend class b2WheelJoint; friend class b2MouseJoint; friend class b2PrismaticJoint; friend class b2PulleyJoint; friend class b2RevoluteJoint; friend class b2WeldJoint; friend class b2FrictionJoint; + friend class b2RopeJoint; // m_flags enum @@ -389,7 +403,7 @@ private: e_bulletFlag = 0x0008, e_fixedRotationFlag = 0x0010, e_activeFlag = 0x0020, - e_toiFlag = 0x0040, + e_toiFlag = 0x0040 }; b2Body(const b2BodyDef* bd, b2World* world); @@ -436,6 +450,7 @@ private: float32 m_linearDamping; float32 m_angularDamping; + float32 m_gravityScale; float32 m_sleepTime; @@ -454,12 +469,12 @@ inline const b2Transform& b2Body::GetTransform() const inline const b2Vec2& b2Body::GetPosition() const { - return m_xf.position; + return m_xf.p; } inline float32 b2Body::GetAngle() const { - return m_sweep.a; + return m_xf.q.GetAngle(); } inline const b2Vec2& b2Body::GetWorldCenter() const @@ -536,7 +551,7 @@ inline b2Vec2 b2Body::GetWorldPoint(const b2Vec2& localPoint) const inline b2Vec2 b2Body::GetWorldVector(const b2Vec2& localVector) const { - return b2Mul(m_xf.R, localVector); + return b2Mul(m_xf.q, localVector); } inline b2Vec2 b2Body::GetLocalPoint(const b2Vec2& worldPoint) const @@ -546,7 +561,7 @@ inline b2Vec2 b2Body::GetLocalPoint(const b2Vec2& worldPoint) const inline b2Vec2 b2Body::GetLocalVector(const b2Vec2& worldVector) const { - return b2MulT(m_xf.R, worldVector); + return b2MulT(m_xf.q, worldVector); } inline b2Vec2 b2Body::GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const @@ -579,6 +594,16 @@ inline void b2Body::SetAngularDamping(float32 angularDamping) m_angularDamping = angularDamping; } +inline float32 b2Body::GetGravityScale() const +{ + return m_gravityScale; +} + +inline void b2Body::SetGravityScale(float32 scale) +{ + m_gravityScale = scale; +} + inline void b2Body::SetBullet(bool flag) { if (flag) @@ -730,6 +755,21 @@ inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point) m_torque += b2Cross(point - m_sweep.c, force); } +inline void b2Body::ApplyForceToCenter(const b2Vec2& force) +{ + if (m_type != b2_dynamicBody) + { + return; + } + + if (IsAwake() == false) + { + SetAwake(true); + } + + m_force += force; +} + inline void b2Body::ApplyTorque(float32 torque) { if (m_type != b2_dynamicBody) @@ -776,17 +816,18 @@ inline void b2Body::ApplyAngularImpulse(float32 impulse) inline void b2Body::SynchronizeTransform() { - m_xf.R.Set(m_sweep.a); - m_xf.position = m_sweep.c - b2Mul(m_xf.R, m_sweep.localCenter); + m_xf.q.Set(m_sweep.a); + m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter); } -inline void b2Body::Advance(float32 t) +inline void b2Body::Advance(float32 alpha) { - // Advance to the new safe time. - m_sweep.Advance(t); + // Advance to the new safe time. This doesn't sync the broad-phase. + m_sweep.Advance(alpha); m_sweep.c = m_sweep.c0; m_sweep.a = m_sweep.a0; - SynchronizeTransform(); + m_xf.q.Set(m_sweep.a); + m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter); } inline b2World* b2Body::GetWorld() diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2ContactManager.cpp b/src/modules/physics/box2d/Box2D/Dynamics/b2ContactManager.cpp old mode 100644 new mode 100755 index d8d96ddbc..ee12b325e --- a/src/modules/physics/box2d/Box2D/Dynamics/b2ContactManager.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2ContactManager.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -110,10 +110,16 @@ void b2ContactManager::Collide() { b2Fixture* fixtureA = c->GetFixtureA(); b2Fixture* fixtureB = c->GetFixtureB(); + int32 indexA = c->GetChildIndexA(); + int32 indexB = c->GetChildIndexB(); b2Body* bodyA = fixtureA->GetBody(); b2Body* bodyB = fixtureB->GetBody(); + + bool activeA = bodyA->IsAwake() && bodyA->m_type != b2_staticBody; + bool activeB = bodyB->IsAwake() && bodyB->m_type != b2_staticBody; - if (bodyA->IsAwake() == false && bodyB->IsAwake() == false) + // At least one body must be awake and it must be dynamic or kinematic. + if (activeA == false && activeB == false) { c = c->GetNext(); continue; @@ -144,8 +150,8 @@ void b2ContactManager::Collide() c->m_flags &= ~b2Contact::e_filterFlag; } - int32 proxyIdA = fixtureA->m_proxyId; - int32 proxyIdB = fixtureB->m_proxyId; + int32 proxyIdA = fixtureA->m_proxies[indexA].proxyId; + int32 proxyIdB = fixtureB->m_proxies[indexB].proxyId; bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB); // Here we destroy contacts that cease to overlap in the broad-phase. @@ -170,8 +176,14 @@ void b2ContactManager::FindNewContacts() void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) { - b2Fixture* fixtureA = (b2Fixture*)proxyUserDataA; - b2Fixture* fixtureB = (b2Fixture*)proxyUserDataB; + b2FixtureProxy* proxyA = (b2FixtureProxy*)proxyUserDataA; + b2FixtureProxy* proxyB = (b2FixtureProxy*)proxyUserDataB; + + b2Fixture* fixtureA = proxyA->fixture; + b2Fixture* fixtureB = proxyB->fixture; + + int32 indexA = proxyA->childIndex; + int32 indexB = proxyB->childIndex; b2Body* bodyA = fixtureA->GetBody(); b2Body* bodyB = fixtureB->GetBody(); @@ -182,6 +194,8 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) return; } + // TODO_ERIN use a hash table to remove a potential bottleneck when both + // bodies have a lot of contacts. // Does a contact already exist? b2ContactEdge* edge = bodyB->GetContactList(); while (edge) @@ -190,13 +204,16 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) { b2Fixture* fA = edge->contact->GetFixtureA(); b2Fixture* fB = edge->contact->GetFixtureB(); - if (fA == fixtureA && fB == fixtureB) + int32 iA = edge->contact->GetChildIndexA(); + int32 iB = edge->contact->GetChildIndexB(); + + if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB) { // A contact already exists. return; } - if (fA == fixtureB && fB == fixtureA) + if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA) { // A contact already exists. return; @@ -219,11 +236,17 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) } // Call the factory. - b2Contact* c = b2Contact::Create(fixtureA, fixtureB, m_allocator); + b2Contact* c = b2Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator); + if (c == NULL) + { + return; + } // Contact creation may swap fixtures. fixtureA = c->GetFixtureA(); fixtureB = c->GetFixtureB(); + indexA = c->GetChildIndexA(); + indexB = c->GetChildIndexB(); bodyA = fixtureA->GetBody(); bodyB = fixtureB->GetBody(); @@ -262,5 +285,9 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) } bodyB->m_contactList = &c->m_nodeB; + // Wake up the bodies + bodyA->SetAwake(true); + bodyB->SetAwake(true); + ++m_contactCount; } diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2ContactManager.h b/src/modules/physics/box2d/Box2D/Dynamics/b2ContactManager.h old mode 100644 new mode 100755 index fcba54ae7..dc1f77fe5 --- a/src/modules/physics/box2d/Box2D/Dynamics/b2ContactManager.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2ContactManager.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.cpp b/src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.cpp old mode 100644 new mode 100755 index 31355b572..478088a44 --- a/src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -18,29 +18,26 @@ #include #include +#include #include +#include #include +#include #include #include #include - b2Fixture::b2Fixture() { m_userData = NULL; m_body = NULL; m_next = NULL; - m_proxyId = b2BroadPhase::e_nullProxy; + m_proxies = NULL; + m_proxyCount = 0; m_shape = NULL; m_density = 0.0f; } -b2Fixture::~b2Fixture() -{ - b2Assert(m_shape == NULL); - b2Assert(m_proxyId == b2BroadPhase::e_nullProxy); -} - void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def) { m_userData = def->userData; @@ -56,13 +53,28 @@ void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2Fixtur m_shape = def->shape->Clone(allocator); + // Reserve proxy space + int32 childCount = m_shape->GetChildCount(); + m_proxies = (b2FixtureProxy*)allocator->Allocate(childCount * sizeof(b2FixtureProxy)); + for (int32 i = 0; i < childCount; ++i) + { + m_proxies[i].fixture = NULL; + m_proxies[i].proxyId = b2BroadPhase::e_nullProxy; + } + m_proxyCount = 0; + m_density = def->density; } void b2Fixture::Destroy(b2BlockAllocator* allocator) { - // The proxy must be destroyed before calling this. - b2Assert(m_proxyId == b2BroadPhase::e_nullProxy); + // The proxies must be destroyed before calling this. + b2Assert(m_proxyCount == 0); + + // Free the proxy array. + int32 childCount = m_shape->GetChildCount(); + allocator->Free(m_proxies, childCount * sizeof(b2FixtureProxy)); + m_proxies = NULL; // Free the child shape. switch (m_shape->m_type) @@ -75,6 +87,14 @@ void b2Fixture::Destroy(b2BlockAllocator* allocator) } break; + case b2Shape::e_edge: + { + b2EdgeShape* s = (b2EdgeShape*)m_shape; + s->~b2EdgeShape(); + allocator->Free(s, sizeof(b2EdgeShape)); + } + break; + case b2Shape::e_polygon: { b2PolygonShape* s = (b2PolygonShape*)m_shape; @@ -83,6 +103,14 @@ void b2Fixture::Destroy(b2BlockAllocator* allocator) } break; + case b2Shape::e_chain: + { + b2ChainShape* s = (b2ChainShape*)m_shape; + s->~b2ChainShape(); + allocator->Free(s, sizeof(b2ChainShape)); + } + break; + default: b2Assert(false); break; @@ -91,50 +119,69 @@ void b2Fixture::Destroy(b2BlockAllocator* allocator) m_shape = NULL; } -void b2Fixture::CreateProxy(b2BroadPhase* broadPhase, const b2Transform& xf) +void b2Fixture::CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf) { - b2Assert(m_proxyId == b2BroadPhase::e_nullProxy); + b2Assert(m_proxyCount == 0); - // Create proxy in the broad-phase. - m_shape->ComputeAABB(&m_aabb, xf); - m_proxyId = broadPhase->CreateProxy(m_aabb, this); + // Create proxies in the broad-phase. + m_proxyCount = m_shape->GetChildCount(); + + for (int32 i = 0; i < m_proxyCount; ++i) + { + b2FixtureProxy* proxy = m_proxies + i; + m_shape->ComputeAABB(&proxy->aabb, xf, i); + proxy->proxyId = broadPhase->CreateProxy(proxy->aabb, proxy); + proxy->fixture = this; + proxy->childIndex = i; + } } -void b2Fixture::DestroyProxy(b2BroadPhase* broadPhase) +void b2Fixture::DestroyProxies(b2BroadPhase* broadPhase) { - if (m_proxyId == b2BroadPhase::e_nullProxy) + // Destroy proxies in the broad-phase. + for (int32 i = 0; i < m_proxyCount; ++i) { - return; + b2FixtureProxy* proxy = m_proxies + i; + broadPhase->DestroyProxy(proxy->proxyId); + proxy->proxyId = b2BroadPhase::e_nullProxy; } - // Destroy proxy in the broad-phase. - broadPhase->DestroyProxy(m_proxyId); - m_proxyId = b2BroadPhase::e_nullProxy; + m_proxyCount = 0; } void b2Fixture::Synchronize(b2BroadPhase* broadPhase, const b2Transform& transform1, const b2Transform& transform2) { - if (m_proxyId == b2BroadPhase::e_nullProxy) + if (m_proxyCount == 0) { return; } - // Compute an AABB that covers the swept shape (may miss some rotation effect). - b2AABB aabb1, aabb2; - m_shape->ComputeAABB(&aabb1, transform1); - m_shape->ComputeAABB(&aabb2, transform2); + for (int32 i = 0; i < m_proxyCount; ++i) + { + b2FixtureProxy* proxy = m_proxies + i; + + // Compute an AABB that covers the swept shape (may miss some rotation effect). + b2AABB aabb1, aabb2; + m_shape->ComputeAABB(&aabb1, transform1, proxy->childIndex); + m_shape->ComputeAABB(&aabb2, transform2, proxy->childIndex); - m_aabb.Combine(aabb1, aabb2); + proxy->aabb.Combine(aabb1, aabb2); - b2Vec2 displacement = transform2.position - transform1.position; + b2Vec2 displacement = transform2.p - transform1.p; - broadPhase->MoveProxy(m_proxyId, m_aabb, displacement); + broadPhase->MoveProxy(proxy->proxyId, proxy->aabb, displacement); + } } void b2Fixture::SetFilterData(const b2Filter& filter) { m_filter = filter; + Refilter(); +} + +void b2Fixture::Refilter() +{ if (m_body == NULL) { return; @@ -154,10 +201,28 @@ void b2Fixture::SetFilterData(const b2Filter& filter) edge = edge->next; } + + b2World* world = m_body->GetWorld(); + + if (world == NULL) + { + return; + } + + // Touch each proxy so that new pairs may be created + b2BroadPhase* broadPhase = &world->m_contactManager.m_broadPhase; + for (int32 i = 0; i < m_proxyCount; ++i) + { + broadPhase->TouchProxy(m_proxies[i].proxyId); + } } void b2Fixture::SetSensor(bool sensor) { - m_isSensor = sensor; + if (sensor != m_isSensor) + { + m_body->SetAwake(true); + m_isSensor = sensor; + } } diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.h b/src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.h old mode 100644 new mode 100755 index d9d325554..a7b5c3428 --- a/src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -26,6 +26,7 @@ class b2BlockAllocator; class b2Body; class b2BroadPhase; +class b2Fixture; /// This holds contact filtering data. struct b2Filter @@ -61,8 +62,6 @@ struct b2FixtureDef isSensor = false; } - virtual ~b2FixtureDef() {} - /// The shape, this must be set. The shape will be cloned, so you /// can create the shape on the stack. const b2Shape* shape; @@ -87,6 +86,14 @@ struct b2FixtureDef b2Filter filter; }; +/// This proxy is used internally to connect fixtures to the broad-phase. +struct b2FixtureProxy +{ + b2AABB aabb; + b2Fixture* fixture; + int32 childIndex; + int32 proxyId; +}; /// A fixture is used to attach a shape to a body for collision detection. A fixture /// inherits its transform from its parent. Fixtures hold additional non-geometric data @@ -115,11 +122,15 @@ public: /// Set the contact filtering data. This will not update contacts until the next time /// step when either parent body is active and awake. + /// This automatically calls Refilter. void SetFilterData(const b2Filter& filter); /// Get the contact filtering data. const b2Filter& GetFilterData() const; + /// Call this if you want to establish collision that was previously disabled by b2ContactFilter::ShouldCollide. + void Refilter(); + /// Get the parent body of this fixture. This is NULL if the fixture is not attached. /// @return the parent body. b2Body* GetBody(); @@ -138,14 +149,13 @@ public: void SetUserData(void* data); /// Test a point for containment in this fixture. - /// @param xf the shape world transform. /// @param p a point in world coordinates. bool TestPoint(const b2Vec2& p) const; /// Cast a ray against this shape. /// @param output the ray-cast results. /// @param input the ray-cast input parameters. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const; + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const; /// Get the mass data for this fixture. The mass data is based on the density and /// the shape. The rotational inertia is about the shape's origin. This operation @@ -162,19 +172,21 @@ public: /// Get the coefficient of friction. float32 GetFriction() const; - /// Set the coefficient of friction. + /// Set the coefficient of friction. This will _not_ change the friction of + /// existing contacts. void SetFriction(float32 friction); /// Get the coefficient of restitution. float32 GetRestitution() const; - /// Set the coefficient of restitution. + /// Set the coefficient of restitution. This will _not_ change the restitution of + /// existing contacts. void SetRestitution(float32 restitution); /// Get the fixture's AABB. This AABB may be enlarge and/or stale. /// If you need a more accurate AABB, compute it using the shape and /// the body transform. - const b2AABB& GetAABB() const; + const b2AABB& GetAABB(int32 childIndex) const; protected: @@ -184,7 +196,6 @@ protected: friend class b2ContactManager; b2Fixture(); - ~b2Fixture(); // We need separation create/destroy functions from the constructor/destructor because // the destructor cannot access the allocator (no destructor arguments allowed by C++). @@ -192,13 +203,11 @@ protected: void Destroy(b2BlockAllocator* allocator); // These support body activation/deactivation. - void CreateProxy(b2BroadPhase* broadPhase, const b2Transform& xf); - void DestroyProxy(b2BroadPhase* broadPhase); + void CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf); + void DestroyProxies(b2BroadPhase* broadPhase); void Synchronize(b2BroadPhase* broadPhase, const b2Transform& xf1, const b2Transform& xf2); - b2AABB m_aabb; - float32 m_density; b2Fixture* m_next; @@ -209,7 +218,9 @@ protected: float32 m_friction; float32 m_restitution; - int32 m_proxyId; + b2FixtureProxy* m_proxies; + int32 m_proxyCount; + b2Filter m_filter; bool m_isSensor; @@ -308,9 +319,9 @@ inline bool b2Fixture::TestPoint(const b2Vec2& p) const return m_shape->TestPoint(m_body->GetTransform(), p); } -inline bool b2Fixture::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const +inline bool b2Fixture::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const { - return m_shape->RayCast(output, input, m_body->GetTransform()); + return m_shape->RayCast(output, input, m_body->GetTransform(), childIndex); } inline void b2Fixture::GetMassData(b2MassData* massData) const @@ -318,9 +329,10 @@ inline void b2Fixture::GetMassData(b2MassData* massData) const m_shape->ComputeMass(massData, m_density); } -inline const b2AABB& b2Fixture::GetAABB() const +inline const b2AABB& b2Fixture::GetAABB(int32 childIndex) const { - return m_aabb; + b2Assert(0 <= childIndex && childIndex < m_proxyCount); + return m_proxies[childIndex].aabb; } #endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2Island.cpp b/src/modules/physics/box2d/Box2D/Dynamics/b2Island.cpp old mode 100644 new mode 100755 index 009249979..0e2129cf8 --- a/src/modules/physics/box2d/Box2D/Dynamics/b2Island.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2Island.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -16,6 +16,7 @@ * 3. This notice may not be removed or altered from any source distribution. */ +#include #include #include #include @@ -24,6 +25,7 @@ #include #include #include +#include /* Position Correction Notes @@ -178,130 +180,166 @@ b2Island::~b2Island() m_allocator->Free(m_bodies); } -void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep) +void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep) { - // Integrate velocities and apply damping. + b2Timer timer; + + float32 h = step.dt; + + // Integrate velocities and apply damping. Initialize the body state. for (int32 i = 0; i < m_bodyCount; ++i) { b2Body* b = m_bodies[i]; - if (b->GetType() != b2_dynamicBody) - { - continue; - } - - // Integrate velocities. - b->m_linearVelocity += step.dt * (gravity + b->m_invMass * b->m_force); - b->m_angularVelocity += step.dt * b->m_invI * b->m_torque; - - // Apply damping. - // ODE: dv/dt + c * v = 0 - // Solution: v(t) = v0 * exp(-c * t) - // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) - // v2 = exp(-c * dt) * v1 - // Taylor expansion: - // v2 = (1.0f - c * dt) * v1 - b->m_linearVelocity *= b2Clamp(1.0f - step.dt * b->m_linearDamping, 0.0f, 1.0f); - b->m_angularVelocity *= b2Clamp(1.0f - step.dt * b->m_angularDamping, 0.0f, 1.0f); - } - - // Partition contacts so that contacts with static bodies are solved last. - int32 i1 = -1; - for (int32 i2 = 0; i2 < m_contactCount; ++i2) - { - b2Fixture* fixtureA = m_contacts[i2]->GetFixtureA(); - b2Fixture* fixtureB = m_contacts[i2]->GetFixtureB(); - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - bool nonStatic = bodyA->GetType() != b2_staticBody && bodyB->GetType() != b2_staticBody; - if (nonStatic) - { - ++i1; - b2Swap(m_contacts[i1], m_contacts[i2]); - } - } - - // Initialize velocity constraints. - b2ContactSolver contactSolver(m_contacts, m_contactCount, m_allocator, step.dtRatio); - contactSolver.WarmStart(); - for (int32 i = 0; i < m_jointCount; ++i) - { - m_joints[i]->InitVelocityConstraints(step); - } - - // Solve velocity constraints. - for (int32 i = 0; i < step.velocityIterations; ++i) - { - for (int32 j = 0; j < m_jointCount; ++j) - { - m_joints[j]->SolveVelocityConstraints(step); - } - - contactSolver.SolveVelocityConstraints(); - } - - // Post-solve (store impulses for warm starting). - contactSolver.StoreImpulses(); - - // Integrate positions. - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - - if (b->GetType() == b2_staticBody) - { - continue; - } - - // Check for large velocities. - b2Vec2 translation = step.dt * b->m_linearVelocity; - if (b2Dot(translation, translation) > b2_maxTranslationSquared) - { - float32 ratio = b2_maxTranslation / translation.Length(); - b->m_linearVelocity *= ratio; - } - - float32 rotation = step.dt * b->m_angularVelocity; - if (rotation * rotation > b2_maxRotationSquared) - { - float32 ratio = b2_maxRotation / b2Abs(rotation); - b->m_angularVelocity *= ratio; - } + b2Vec2 c = b->m_sweep.c; + float32 a = b->m_sweep.a; + b2Vec2 v = b->m_linearVelocity; + float32 w = b->m_angularVelocity; // Store positions for continuous collision. b->m_sweep.c0 = b->m_sweep.c; b->m_sweep.a0 = b->m_sweep.a; - // Integrate - b->m_sweep.c += step.dt * b->m_linearVelocity; - b->m_sweep.a += step.dt * b->m_angularVelocity; + if (b->m_type == b2_dynamicBody) + { + // Integrate velocities. + v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force); + w += h * b->m_invI * b->m_torque; - // Compute new transform - b->SynchronizeTransform(); + // Apply damping. + // ODE: dv/dt + c * v = 0 + // Solution: v(t) = v0 * exp(-c * t) + // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) + // v2 = exp(-c * dt) * v1 + // Taylor expansion: + // v2 = (1.0f - c * dt) * v1 + v *= b2Clamp(1.0f - h * b->m_linearDamping, 0.0f, 1.0f); + w *= b2Clamp(1.0f - h * b->m_angularDamping, 0.0f, 1.0f); + } - // Note: shapes are synchronized later. + m_positions[i].c = c; + m_positions[i].a = a; + m_velocities[i].v = v; + m_velocities[i].w = w; } - // Iterate over constraints. + timer.Reset(); + + // Solver data + b2SolverData solverData; + solverData.step = step; + solverData.positions = m_positions; + solverData.velocities = m_velocities; + + // Initialize velocity constraints. + b2ContactSolverDef contactSolverDef; + contactSolverDef.step = step; + contactSolverDef.contacts = m_contacts; + contactSolverDef.count = m_contactCount; + contactSolverDef.positions = m_positions; + contactSolverDef.velocities = m_velocities; + contactSolverDef.allocator = m_allocator; + + b2ContactSolver contactSolver(&contactSolverDef); + contactSolver.InitializeVelocityConstraints(); + + if (step.warmStarting) + { + contactSolver.WarmStart(); + } + + for (int32 i = 0; i < m_jointCount; ++i) + { + m_joints[i]->InitVelocityConstraints(solverData); + } + + profile->solveInit = timer.GetMilliseconds(); + + // Solve velocity constraints + timer.Reset(); + for (int32 i = 0; i < step.velocityIterations; ++i) + { + for (int32 j = 0; j < m_jointCount; ++j) + { + m_joints[j]->SolveVelocityConstraints(solverData); + } + + contactSolver.SolveVelocityConstraints(); + } + + // Store impulses for warm starting + contactSolver.StoreImpulses(); + profile->solveVelocity = timer.GetMilliseconds(); + + // Integrate positions + for (int32 i = 0; i < m_bodyCount; ++i) + { + b2Vec2 c = m_positions[i].c; + float32 a = m_positions[i].a; + b2Vec2 v = m_velocities[i].v; + float32 w = m_velocities[i].w; + + // Check for large velocities + b2Vec2 translation = h * v; + if (b2Dot(translation, translation) > b2_maxTranslationSquared) + { + float32 ratio = b2_maxTranslation / translation.Length(); + v *= ratio; + } + + float32 rotation = h * w; + if (rotation * rotation > b2_maxRotationSquared) + { + float32 ratio = b2_maxRotation / b2Abs(rotation); + w *= ratio; + } + + // Integrate + c += h * v; + a += h * w; + + m_positions[i].c = c; + m_positions[i].a = a; + m_velocities[i].v = v; + m_velocities[i].w = w; + } + + // Solve position constraints + timer.Reset(); + bool positionSolved = false; for (int32 i = 0; i < step.positionIterations; ++i) { - bool contactsOkay = contactSolver.SolvePositionConstraints(b2_contactBaumgarte); + bool contactsOkay = contactSolver.SolvePositionConstraints(); bool jointsOkay = true; for (int32 i = 0; i < m_jointCount; ++i) { - bool jointOkay = m_joints[i]->SolvePositionConstraints(b2_contactBaumgarte); + bool jointOkay = m_joints[i]->SolvePositionConstraints(solverData); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. + positionSolved = true; break; } } - Report(contactSolver.m_constraints); + // Copy state buffers back to the bodies + for (int32 i = 0; i < m_bodyCount; ++i) + { + b2Body* body = m_bodies[i]; + body->m_sweep.c = m_positions[i].c; + body->m_sweep.a = m_positions[i].a; + body->m_linearVelocity = m_velocities[i].v; + body->m_angularVelocity = m_velocities[i].w; + body->SynchronizeTransform(); + } + + profile->solvePosition = timer.GetMilliseconds(); + + Report(contactSolver.m_velocityConstraints); if (allowSleep) { @@ -318,12 +356,6 @@ void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSl continue; } - if ((b->m_flags & b2Body::e_autoSleepFlag) == 0) - { - b->m_sleepTime = 0.0f; - minSleepTime = 0.0f; - } - if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 || b->m_angularVelocity * b->m_angularVelocity > angTolSqr || b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr) @@ -333,12 +365,12 @@ void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSl } else { - b->m_sleepTime += step.dt; + b->m_sleepTime += h; minSleepTime = b2Min(minSleepTime, b->m_sleepTime); } } - if (minSleepTime >= b2_timeToSleep) + if (minSleepTime >= b2_timeToSleep && positionSolved) { for (int32 i = 0; i < m_bodyCount; ++i) { @@ -349,7 +381,139 @@ void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSl } } -void b2Island::Report(const b2ContactConstraint* constraints) +void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB) +{ + b2Assert(toiIndexA < m_bodyCount); + b2Assert(toiIndexB < m_bodyCount); + + // Initialize the body state. + for (int32 i = 0; i < m_bodyCount; ++i) + { + b2Body* b = m_bodies[i]; + m_positions[i].c = b->m_sweep.c; + m_positions[i].a = b->m_sweep.a; + m_velocities[i].v = b->m_linearVelocity; + m_velocities[i].w = b->m_angularVelocity; + } + + b2ContactSolverDef contactSolverDef; + contactSolverDef.contacts = m_contacts; + contactSolverDef.count = m_contactCount; + contactSolverDef.allocator = m_allocator; + contactSolverDef.step = subStep; + contactSolverDef.positions = m_positions; + contactSolverDef.velocities = m_velocities; + b2ContactSolver contactSolver(&contactSolverDef); + + // Solve position constraints. + for (int32 i = 0; i < subStep.positionIterations; ++i) + { + bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); + if (contactsOkay) + { + break; + } + } + +#if 0 + // Is the new position really safe? + for (int32 i = 0; i < m_contactCount; ++i) + { + b2Contact* c = m_contacts[i]; + b2Fixture* fA = c->GetFixtureA(); + b2Fixture* fB = c->GetFixtureB(); + + b2Body* bA = fA->GetBody(); + b2Body* bB = fB->GetBody(); + + int32 indexA = c->GetChildIndexA(); + int32 indexB = c->GetChildIndexB(); + + b2DistanceInput input; + input.proxyA.Set(fA->GetShape(), indexA); + input.proxyB.Set(fB->GetShape(), indexB); + input.transformA = bA->GetTransform(); + input.transformB = bB->GetTransform(); + input.useRadii = false; + + b2DistanceOutput output; + b2SimplexCache cache; + cache.count = 0; + b2Distance(&output, &cache, &input); + + if (output.distance == 0 || cache.count == 3) + { + cache.count += 0; + } + } +#endif + + // Leap of faith to new safe state. + m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c; + m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a; + m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c; + m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a; + + // No warm starting is needed for TOI events because warm + // starting impulses were applied in the discrete solver. + contactSolver.InitializeVelocityConstraints(); + + // Solve velocity constraints. + for (int32 i = 0; i < subStep.velocityIterations; ++i) + { + contactSolver.SolveVelocityConstraints(); + } + + // Don't store the TOI contact forces for warm starting + // because they can be quite large. + + float32 h = subStep.dt; + + // Integrate positions + for (int32 i = 0; i < m_bodyCount; ++i) + { + b2Vec2 c = m_positions[i].c; + float32 a = m_positions[i].a; + b2Vec2 v = m_velocities[i].v; + float32 w = m_velocities[i].w; + + // Check for large velocities + b2Vec2 translation = h * v; + if (b2Dot(translation, translation) > b2_maxTranslationSquared) + { + float32 ratio = b2_maxTranslation / translation.Length(); + v *= ratio; + } + + float32 rotation = h * w; + if (rotation * rotation > b2_maxRotationSquared) + { + float32 ratio = b2_maxRotation / b2Abs(rotation); + w *= ratio; + } + + // Integrate + c += h * v; + a += h * w; + + m_positions[i].c = c; + m_positions[i].a = a; + m_velocities[i].v = v; + m_velocities[i].w = w; + + // Sync bodies + b2Body* body = m_bodies[i]; + body->m_sweep.c = c; + body->m_sweep.a = a; + body->m_linearVelocity = v; + body->m_angularVelocity = w; + body->SynchronizeTransform(); + } + + Report(contactSolver.m_velocityConstraints); +} + +void b2Island::Report(const b2ContactVelocityConstraint* constraints) { if (m_listener == NULL) { @@ -360,13 +524,14 @@ void b2Island::Report(const b2ContactConstraint* constraints) { b2Contact* c = m_contacts[i]; - const b2ContactConstraint* cc = constraints + i; + const b2ContactVelocityConstraint* vc = constraints + i; b2ContactImpulse impulse; - for (int32 j = 0; j < cc->pointCount; ++j) + impulse.count = vc->pointCount; + for (int32 j = 0; j < vc->pointCount; ++j) { - impulse.normalImpulses[j] = cc->points[j].normalImpulse; - impulse.tangentImpulses[j] = cc->points[j].tangentImpulse; + impulse.normalImpulses[j] = vc->points[j].normalImpulse; + impulse.tangentImpulses[j] = vc->points[j].tangentImpulse; } m_listener->PostSolve(c, &impulse); diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2Island.h b/src/modules/physics/box2d/Box2D/Dynamics/b2Island.h old mode 100644 new mode 100755 index 3a5fd1131..ef3d9b1df --- a/src/modules/physics/box2d/Box2D/Dynamics/b2Island.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2Island.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -27,21 +27,8 @@ class b2Contact; class b2Joint; class b2StackAllocator; class b2ContactListener; -struct b2ContactConstraint; - -/// This is an internal structure. -struct b2Position -{ - b2Vec2 x; - float32 a; -}; - -/// This is an internal structure. -struct b2Velocity -{ - b2Vec2 v; - float32 w; -}; +struct b2ContactVelocityConstraint; +struct b2Profile; /// This is an internal class. class b2Island @@ -58,13 +45,16 @@ public: m_jointCount = 0; } - void Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep); + void Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep); + + void SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB); void Add(b2Body* body) { b2Assert(m_bodyCount < m_bodyCapacity); body->m_islandIndex = m_bodyCount; - m_bodies[m_bodyCount++] = body; + m_bodies[m_bodyCount] = body; + ++m_bodyCount; } void Add(b2Contact* contact) @@ -79,7 +69,7 @@ public: m_joints[m_jointCount++] = joint; } - void Report(const b2ContactConstraint* constraints); + void Report(const b2ContactVelocityConstraint* constraints); b2StackAllocator* m_allocator; b2ContactListener* m_listener; @@ -98,8 +88,6 @@ public: int32 m_bodyCapacity; int32 m_contactCapacity; int32 m_jointCapacity; - - int32 m_positionIterationCount; }; #endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2TimeStep.h b/src/modules/physics/box2d/Box2D/Dynamics/b2TimeStep.h old mode 100644 new mode 100755 index f59245dd6..80916cb65 --- a/src/modules/physics/box2d/Box2D/Dynamics/b2TimeStep.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2TimeStep.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -19,17 +19,52 @@ #ifndef B2_TIME_STEP_H #define B2_TIME_STEP_H -#include +#include -/// This is an internal structure. -struct b2TimeStep +/// Profiling data. Times are in milliseconds. +struct b2Profile { - float32 dt; // time step - float32 inv_dt; // inverse time step (0 if dt == 0). - float32 dtRatio; // dt * inv_dt0 - int32 velocityIterations; - int32 positionIterations; - bool warmStarting; + float32 step; + float32 collide; + float32 solve; + float32 solveInit; + float32 solveVelocity; + float32 solvePosition; + float32 broadphase; + float32 solveTOI; }; +/// This is an internal structure. +struct b2TimeStep +{ + float32 dt; // time step + float32 inv_dt; // inverse time step (0 if dt == 0). + float32 dtRatio; // dt * inv_dt0 + int32 velocityIterations; + int32 positionIterations; + bool warmStarting; +}; + +/// This is an internal structure. +struct b2Position +{ + b2Vec2 c; + float32 a; +}; + +/// This is an internal structure. +struct b2Velocity +{ + b2Vec2 v; + float32 w; +}; + +/// Solver Data +struct b2SolverData +{ + b2TimeStep step; + b2Position* positions; + b2Velocity* velocities; +}; + #endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2World.cpp b/src/modules/physics/box2d/Box2D/Dynamics/b2World.cpp old mode 100644 new mode 100755 index d3b9e0038..7dcddd03c --- a/src/modules/physics/box2d/Box2D/Dynamics/b2World.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2World.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -23,12 +23,15 @@ #include #include #include -#include #include #include #include +#include +#include #include #include +#include +#include #include b2World::b2World(const b2Vec2& gravity, bool doSleep) @@ -44,6 +47,9 @@ b2World::b2World(const b2Vec2& gravity, bool doSleep) m_warmStarting = true; m_continuousPhysics = true; + m_subStepping = false; + + m_stepComplete = true; m_allowSleep = doSleep; m_gravity = gravity; @@ -53,10 +59,29 @@ b2World::b2World(const b2Vec2& gravity, bool doSleep) m_inv_dt0 = 0.0f; m_contactManager.m_allocator = &m_blockAllocator; + + memset(&m_profile, 0, sizeof(b2Profile)); } b2World::~b2World() { + // Some shapes allocate using b2Alloc. + b2Body* b = m_bodyList; + while (b) + { + b2Body* bNext = b->m_next; + + b2Fixture* f = b->m_fixtureList; + while (f) + { + b2Fixture* fNext = f->m_next; + f->m_proxyCount = 0; + f->Destroy(&m_blockAllocator); + f = fNext; + } + + b = bNext; + } } void b2World::SetDestructionListener(b2DestructionListener* listener) @@ -74,7 +99,7 @@ void b2World::SetContactListener(b2ContactListener* listener) m_contactManager.m_contactListener = listener; } -void b2World::SetDebugDraw(b2DebugDraw* debugDraw) +void b2World::SetDebugDraw(b2Draw* debugDraw) { m_debugDraw = debugDraw; } @@ -125,6 +150,8 @@ void b2World::DestroyBody(b2Body* b) } DestroyJoint(je0->joint); + + b->m_jointList = je; } b->m_jointList = NULL; @@ -150,10 +177,13 @@ void b2World::DestroyBody(b2Body* b) m_destructionListener->SayGoodbye(f0); } - f0->DestroyProxy(&m_contactManager.m_broadPhase); + f0->DestroyProxies(&m_contactManager.m_broadPhase); f0->Destroy(&m_blockAllocator); f0->~b2Fixture(); m_blockAllocator.Free(f0, sizeof(b2Fixture)); + + b->m_fixtureList = f; + b->m_fixtureCount -= 1; } b->m_fixtureList = NULL; b->m_fixtureCount = 0; @@ -337,6 +367,10 @@ void b2World::DestroyJoint(b2Joint* j) // Find islands, integrate and solve constraints, solve position constraints void b2World::Solve(const b2TimeStep& step) { + m_profile.solveInit = 0.0f; + m_profile.solveVelocity = 0.0f; + m_profile.solvePosition = 0.0f; + // Size the island for the worst case. b2Island island(m_bodyCount, m_contactManager.m_contactCount, @@ -475,7 +509,11 @@ void b2World::Solve(const b2TimeStep& step) } } - island.Solve(step, m_gravity, m_allowSleep); + b2Profile profile; + island.Solve(&profile, step, m_gravity, m_allowSleep); + m_profile.solveInit += profile.solveInit; + m_profile.solveVelocity += profile.solveVelocity; + m_profile.solvePosition += profile.solvePosition; // Post solve cleanup. for (int32 i = 0; i < island.m_bodyCount; ++i) @@ -491,292 +529,357 @@ void b2World::Solve(const b2TimeStep& step) m_stackAllocator.Free(stack); - // Synchronize fixtures, check for out of range bodies. - for (b2Body* b = m_bodyList; b; b = b->GetNext()) { - // If a body was not in an island then it did not move. - if ((b->m_flags & b2Body::e_islandFlag) == 0) + b2Timer timer; + // Synchronize fixtures, check for out of range bodies. + for (b2Body* b = m_bodyList; b; b = b->GetNext()) { - continue; - } - - if (b->GetType() == b2_staticBody) - { - continue; - } - - // Update fixtures (for broad-phase). - b->SynchronizeFixtures(); - } - - // Look for new contacts. - m_contactManager.FindNewContacts(); -} - -// Advance a dynamic body to its first time of contact -// and adjust the position to ensure clearance. -void b2World::SolveTOI(b2Body* body) -{ - // Find the minimum contact. - b2Contact* toiContact = NULL; - float32 toi = 1.0f; - b2Body* toiOther = NULL; - bool found; - int32 count; - int32 iter = 0; - - bool bullet = body->IsBullet(); - - // Iterate until all contacts agree on the minimum TOI. We have - // to iterate because the TOI algorithm may skip some intermediate - // collisions when objects rotate through each other. - do - { - count = 0; - found = false; - for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) - { - if (ce->contact == toiContact) + // If a body was not in an island then it did not move. + if ((b->m_flags & b2Body::e_islandFlag) == 0) { continue; } - b2Body* other = ce->other; - b2BodyType type = other->GetType(); - - // Only bullets perform TOI with dynamic bodies. - if (bullet == true) + if (b->GetType() == b2_staticBody) { - // Bullets only perform TOI with bodies that have their TOI resolved. - if ((other->m_flags & b2Body::e_toiFlag) == 0) + continue; + } + + // Update fixtures (for broad-phase). + b->SynchronizeFixtures(); + } + + // Look for new contacts. + m_contactManager.FindNewContacts(); + m_profile.broadphase = timer.GetMilliseconds(); + } +} + +// Find TOI contacts and solve them. +void b2World::SolveTOI(const b2TimeStep& step) +{ + b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener); + + if (m_stepComplete) + { + for (b2Body* b = m_bodyList; b; b = b->m_next) + { + b->m_flags &= ~b2Body::e_islandFlag; + b->m_sweep.alpha0 = 0.0f; + } + + for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) + { + // Invalidate TOI + c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); + c->m_toiCount = 0; + c->m_toi = 1.0f; + } + } + + // Find TOI events and solve them. + for (;;) + { + // Find the first TOI. + b2Contact* minContact = NULL; + float32 minAlpha = 1.0f; + + for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) + { + // Is this contact disabled? + if (c->IsEnabled() == false) + { + continue; + } + + // Prevent excessive sub-stepping. + if (c->m_toiCount > b2_maxSubSteps) + { + continue; + } + + float32 alpha = 1.0f; + if (c->m_flags & b2Contact::e_toiFlag) + { + // This contact has a valid cached TOI. + alpha = c->m_toi; + } + else + { + b2Fixture* fA = c->GetFixtureA(); + b2Fixture* fB = c->GetFixtureB(); + + // Is there a sensor? + if (fA->IsSensor() || fB->IsSensor()) { continue; } - // No repeated hits on non-static bodies - if (type != b2_staticBody && (ce->contact->m_flags & b2Contact::e_bulletHitFlag) != 0) + b2Body* bA = fA->GetBody(); + b2Body* bB = fB->GetBody(); + + b2BodyType typeA = bA->m_type; + b2BodyType typeB = bB->m_type; + b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody); + + bool activeA = bA->IsAwake() && typeA != b2_staticBody; + bool activeB = bB->IsAwake() && typeB != b2_staticBody; + + // Is at least one body active (awake and dynamic or kinematic)? + if (activeA == false && activeB == false) { - continue; + continue; } + + bool collideA = bA->IsBullet() || typeA != b2_dynamicBody; + bool collideB = bB->IsBullet() || typeB != b2_dynamicBody; + + // Are these two non-bullet dynamic bodies? + if (collideA == false && collideB == false) + { + continue; + } + + // Compute the TOI for this contact. + // Put the sweeps onto the same time interval. + float32 alpha0 = bA->m_sweep.alpha0; + + if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0) + { + alpha0 = bB->m_sweep.alpha0; + bA->m_sweep.Advance(alpha0); + } + else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0) + { + alpha0 = bA->m_sweep.alpha0; + bB->m_sweep.Advance(alpha0); + } + + b2Assert(alpha0 < 1.0f); + + int32 indexA = c->GetChildIndexA(); + int32 indexB = c->GetChildIndexB(); + + // Compute the time of impact in interval [0, minTOI] + b2TOIInput input; + input.proxyA.Set(fA->GetShape(), indexA); + input.proxyB.Set(fB->GetShape(), indexB); + input.sweepA = bA->m_sweep; + input.sweepB = bB->m_sweep; + input.tMax = 1.0f; + + b2TOIOutput output; + b2TimeOfImpact(&output, &input); + + // Beta is the fraction of the remaining portion of the . + float32 beta = output.t; + if (output.state == b2TOIOutput::e_touching) + { + alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); + } + else + { + alpha = 1.0f; + } + + c->m_toi = alpha; + c->m_flags |= b2Contact::e_toiFlag; } - else if (type == b2_dynamicBody) + + if (alpha < minAlpha) { - continue; + // This is the minimum TOI found so far. + minContact = c; + minAlpha = alpha; } - - // Check for a disabled contact. - b2Contact* contact = ce->contact; - if (contact->IsEnabled() == false) - { - continue; - } - - // Prevent infinite looping. - if (contact->m_toiCount > 10) - { - continue; - } - - b2Fixture* fixtureA = contact->m_fixtureA; - b2Fixture* fixtureB = contact->m_fixtureB; - - // Cull sensors. - if (fixtureA->IsSensor() || fixtureB->IsSensor()) - { - continue; - } - - b2Body* bodyA = fixtureA->m_body; - b2Body* bodyB = fixtureB->m_body; - - // Compute the time of impact in interval [0, minTOI] - b2TOIInput input; - input.proxyA.Set(fixtureA->GetShape()); - input.proxyB.Set(fixtureB->GetShape()); - input.sweepA = bodyA->m_sweep; - input.sweepB = bodyB->m_sweep; - input.tMax = toi; - - b2TOIOutput output; - b2TimeOfImpact(&output, &input); - - if (output.state == b2TOIOutput::e_touching && output.t < toi) - { - toiContact = contact; - toi = output.t; - toiOther = other; - found = true; - } - - ++count; } - ++iter; - } while (found && count > 1 && iter < 50); - - if (toiContact == NULL) - { - body->Advance(1.0f); - return; - } - - b2Sweep backup = body->m_sweep; - body->Advance(toi); - toiContact->Update(m_contactManager.m_contactListener); - if (toiContact->IsEnabled() == false) - { - // Contact disabled. Backup and recurse. - body->m_sweep = backup; - SolveTOI(body); - } - - ++toiContact->m_toiCount; - - // Update all the valid contacts on this body and build a contact island. - b2Contact* contacts[b2_maxTOIContacts]; - count = 0; - for (b2ContactEdge* ce = body->m_contactList; ce && count < b2_maxTOIContacts; ce = ce->next) - { - b2Body* other = ce->other; - b2BodyType type = other->GetType(); - - // Only perform correction with static bodies, so the - // body won't get pushed out of the world. - if (type == b2_dynamicBody) + if (minContact == NULL || 1.0f - 10.0f * b2_epsilon < minAlpha) { - continue; - } - - // Check for a disabled contact. - b2Contact* contact = ce->contact; - if (contact->IsEnabled() == false) - { - continue; - } - - b2Fixture* fixtureA = contact->m_fixtureA; - b2Fixture* fixtureB = contact->m_fixtureB; - - // Cull sensors. - if (fixtureA->IsSensor() || fixtureB->IsSensor()) - { - continue; - } - - // The contact likely has some new contact points. The listener - // gives the user a chance to disable the contact. - if (contact != toiContact) - { - contact->Update(m_contactManager.m_contactListener); - } - - // Did the user disable the contact? - if (contact->IsEnabled() == false) - { - // Skip this contact. - continue; - } - - if (contact->IsTouching() == false) - { - continue; - } - - contacts[count] = contact; - ++count; - } - - // Reduce the TOI body's overlap with the contact island. - b2TOISolver solver(&m_stackAllocator); - solver.Initialize(contacts, count, body); - - const float32 k_toiBaumgarte = 0.75f; - bool solved = false; - for (int32 i = 0; i < 20; ++i) - { - bool contactsOkay = solver.Solve(k_toiBaumgarte); - if (contactsOkay) - { - solved = true; + // No more TOI events. Done! + m_stepComplete = true; break; } - } - if (toiOther->GetType() != b2_staticBody) - { - toiContact->m_flags |= b2Contact::e_bulletHitFlag; - } -} + // Advance the bodies to the TOI. + b2Fixture* fA = minContact->GetFixtureA(); + b2Fixture* fB = minContact->GetFixtureB(); + b2Body* bA = fA->GetBody(); + b2Body* bB = fB->GetBody(); -// Sequentially solve TOIs for each body. We bring each -// body to the time of contact and perform some position correction. -// Time is not conserved. -void b2World::SolveTOI() -{ - // Prepare all contacts. - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) - { - // Enable the contact - c->m_flags |= b2Contact::e_enabledFlag; + b2Sweep backup1 = bA->m_sweep; + b2Sweep backup2 = bB->m_sweep; - // Set the number of TOI events for this contact to zero. - c->m_toiCount = 0; - } + bA->Advance(minAlpha); + bB->Advance(minAlpha); - // Initialize the TOI flag. - for (b2Body* body = m_bodyList; body; body = body->m_next) - { - // Kinematic, and static bodies will not be affected by the TOI event. - // If a body was not in an island then it did not move. - if ((body->m_flags & b2Body::e_islandFlag) == 0 || body->GetType() == b2_kinematicBody || body->GetType() == b2_staticBody) - { - body->m_flags |= b2Body::e_toiFlag; - } - else - { - body->m_flags &= ~b2Body::e_toiFlag; - } - } - - // Collide non-bullets. - for (b2Body* body = m_bodyList; body; body = body->m_next) - { - if (body->m_flags & b2Body::e_toiFlag) + // The TOI contact likely has some new contact points. + minContact->Update(m_contactManager.m_contactListener); + minContact->m_flags &= ~b2Contact::e_toiFlag; + ++minContact->m_toiCount; + + // Is the contact solid? + if (minContact->IsEnabled() == false || minContact->IsTouching() == false) { + // Restore the sweeps. + minContact->SetEnabled(false); + bA->m_sweep = backup1; + bB->m_sweep = backup2; + bA->SynchronizeTransform(); + bB->SynchronizeTransform(); continue; } - if (body->IsBullet() == true) + bA->SetAwake(true); + bB->SetAwake(true); + + // Build the island + island.Clear(); + island.Add(bA); + island.Add(bB); + island.Add(minContact); + + bA->m_flags |= b2Body::e_islandFlag; + bB->m_flags |= b2Body::e_islandFlag; + minContact->m_flags |= b2Contact::e_islandFlag; + + // Get contacts on bodyA and bodyB. + b2Body* bodies[2] = {bA, bB}; + for (int32 i = 0; i < 2; ++i) { - continue; + b2Body* body = bodies[i]; + if (body->m_type == b2_dynamicBody) + { + for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) + { + if (island.m_bodyCount == island.m_bodyCapacity) + { + break; + } + + if (island.m_contactCount == island.m_contactCapacity) + { + break; + } + + b2Contact* contact = ce->contact; + + // Has this contact already been added to the island? + if (contact->m_flags & b2Contact::e_islandFlag) + { + continue; + } + + // Only add static, kinematic, or bullet bodies. + b2Body* other = ce->other; + if (other->m_type == b2_dynamicBody && + body->IsBullet() == false && other->IsBullet() == false) + { + continue; + } + + // Skip sensors. + bool sensorA = contact->m_fixtureA->m_isSensor; + bool sensorB = contact->m_fixtureB->m_isSensor; + if (sensorA || sensorB) + { + continue; + } + + // Tentatively advance the body to the TOI. + b2Sweep backup = other->m_sweep; + if ((other->m_flags & b2Body::e_islandFlag) == 0) + { + other->Advance(minAlpha); + } + + // Update the contact points + contact->Update(m_contactManager.m_contactListener); + + // Was the contact disabled by the user? + if (contact->IsEnabled() == false) + { + other->m_sweep = backup; + other->SynchronizeTransform(); + continue; + } + + // Are there contact points? + if (contact->IsTouching() == false) + { + other->m_sweep = backup; + other->SynchronizeTransform(); + continue; + } + + // Add the contact to the island + contact->m_flags |= b2Contact::e_islandFlag; + island.Add(contact); + + // Has the other body already been added to the island? + if (other->m_flags & b2Body::e_islandFlag) + { + continue; + } + + // Add the other body to the island. + other->m_flags |= b2Body::e_islandFlag; + + if (other->m_type != b2_staticBody) + { + other->SetAwake(true); + } + + island.Add(other); + } + } } - SolveTOI(body); + b2TimeStep subStep; + subStep.dt = (1.0f - minAlpha) * step.dt; + subStep.inv_dt = 1.0f / subStep.dt; + subStep.dtRatio = 1.0f; + subStep.positionIterations = 20; + subStep.velocityIterations = step.velocityIterations; + subStep.warmStarting = false; + island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex); - body->m_flags |= b2Body::e_toiFlag; - } - - // Collide bullets. - for (b2Body* body = m_bodyList; body; body = body->m_next) - { - if (body->m_flags & b2Body::e_toiFlag) + // Reset island flags and synchronize broad-phase proxies. + for (int32 i = 0; i < island.m_bodyCount; ++i) { - continue; + b2Body* body = island.m_bodies[i]; + body->m_flags &= ~b2Body::e_islandFlag; + + if (body->m_type != b2_dynamicBody) + { + continue; + } + + body->SynchronizeFixtures(); + + // Invalidate all contact TOIs on this displaced body. + for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) + { + ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); + } } - if (body->IsBullet() == false) + // Commit fixture proxy movements to the broad-phase so that new contacts are created. + // Also, some contacts can be destroyed. + m_contactManager.FindNewContacts(); + + if (m_subStepping) { - continue; + m_stepComplete = false; + break; } - - SolveTOI(body); - - body->m_flags |= b2Body::e_toiFlag; } } void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations) { + b2Timer stepTimer; + // If new fixtures were added, we need to find the new contacts. if (m_flags & e_newFixture) { @@ -802,20 +905,28 @@ void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIteration step.dtRatio = m_inv_dt0 * dt; step.warmStarting = m_warmStarting; - + // Update contacts. This is where some contacts are destroyed. - m_contactManager.Collide(); + { + b2Timer timer; + m_contactManager.Collide(); + m_profile.collide = timer.GetMilliseconds(); + } // Integrate velocities, solve velocity constraints, and integrate positions. - if (step.dt > 0.0f) + if (m_stepComplete && step.dt > 0.0f) { + b2Timer timer; Solve(step); + m_profile.solve = timer.GetMilliseconds(); } // Handle TOI events. if (m_continuousPhysics && step.dt > 0.0f) { - SolveTOI(); + b2Timer timer; + SolveTOI(step); + m_profile.solveTOI = timer.GetMilliseconds(); } if (step.dt > 0.0f) @@ -829,6 +940,8 @@ void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIteration } m_flags &= ~e_locked; + + m_profile.step = stepTimer.GetMilliseconds(); } void b2World::ClearForces() @@ -844,8 +957,8 @@ struct b2WorldQueryWrapper { bool QueryCallback(int32 proxyId) { - b2Fixture* fixture = (b2Fixture*)broadPhase->GetUserData(proxyId); - return callback->ReportFixture(fixture); + b2FixtureProxy* proxy = (b2FixtureProxy*)broadPhase->GetUserData(proxyId); + return callback->ReportFixture(proxy->fixture); } const b2BroadPhase* broadPhase; @@ -865,9 +978,11 @@ struct b2WorldRayCastWrapper float32 RayCastCallback(const b2RayCastInput& input, int32 proxyId) { void* userData = broadPhase->GetUserData(proxyId); - b2Fixture* fixture = (b2Fixture*)userData; + b2FixtureProxy* proxy = (b2FixtureProxy*)userData; + b2Fixture* fixture = proxy->fixture; + int32 index = proxy->childIndex; b2RayCastOutput output; - bool hit = fixture->RayCast(&output, input); + bool hit = fixture->RayCast(&output, input, index); if (hit) { @@ -905,12 +1020,38 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color b2Vec2 center = b2Mul(xf, circle->m_p); float32 radius = circle->m_radius; - b2Vec2 axis = xf.R.col1; + b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f)); m_debugDraw->DrawSolidCircle(center, radius, axis, color); } break; + case b2Shape::e_edge: + { + b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape(); + b2Vec2 v1 = b2Mul(xf, edge->m_vertex1); + b2Vec2 v2 = b2Mul(xf, edge->m_vertex2); + m_debugDraw->DrawSegment(v1, v2, color); + } + break; + + case b2Shape::e_chain: + { + b2ChainShape* chain = (b2ChainShape*)fixture->GetShape(); + int32 count = chain->GetVertexCount(); + const b2Vec2* vertices = chain->GetVertices(); + + b2Vec2 v1 = b2Mul(xf, vertices[0]); + for (int32 i = 1; i < count; ++i) + { + b2Vec2 v2 = b2Mul(xf, vertices[i]); + m_debugDraw->DrawSegment(v1, v2, color); + m_debugDraw->DrawCircle(v1, 0.05f, color); + v1 = v2; + } + } + break; + case b2Shape::e_polygon: { b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape(); @@ -926,6 +1067,9 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color); } break; + + default: + break; } } @@ -935,8 +1079,8 @@ void b2World::DrawJoint(b2Joint* joint) b2Body* bodyB = joint->GetBodyB(); const b2Transform& xf1 = bodyA->GetTransform(); const b2Transform& xf2 = bodyB->GetTransform(); - b2Vec2 x1 = xf1.position; - b2Vec2 x2 = xf2.position; + b2Vec2 x1 = xf1.p; + b2Vec2 x2 = xf2.p; b2Vec2 p1 = joint->GetAnchorA(); b2Vec2 p2 = joint->GetAnchorB(); @@ -979,7 +1123,7 @@ void b2World::DrawDebugData() uint32 flags = m_debugDraw->GetFlags(); - if (flags & b2DebugDraw::e_shapeBit) + if (flags & b2Draw::e_shapeBit) { for (b2Body* b = m_bodyList; b; b = b->GetNext()) { @@ -1010,7 +1154,7 @@ void b2World::DrawDebugData() } } - if (flags & b2DebugDraw::e_jointBit) + if (flags & b2Draw::e_jointBit) { for (b2Joint* j = m_jointList; j; j = j->GetNext()) { @@ -1018,22 +1162,22 @@ void b2World::DrawDebugData() } } - if (flags & b2DebugDraw::e_pairBit) + if (flags & b2Draw::e_pairBit) { b2Color color(0.3f, 0.9f, 0.9f); for (b2Contact* c = m_contactManager.m_contactList; c; c = c->GetNext()) { - b2Fixture* fixtureA = c->GetFixtureA(); - b2Fixture* fixtureB = c->GetFixtureB(); + //b2Fixture* fixtureA = c->GetFixtureA(); + //b2Fixture* fixtureB = c->GetFixtureB(); - b2Vec2 cA = fixtureA->GetAABB().GetCenter(); - b2Vec2 cB = fixtureB->GetAABB().GetCenter(); + //b2Vec2 cA = fixtureA->GetAABB().GetCenter(); + //b2Vec2 cB = fixtureB->GetAABB().GetCenter(); - m_debugDraw->DrawSegment(cA, cB, color); + //m_debugDraw->DrawSegment(cA, cB, color); } } - if (flags & b2DebugDraw::e_aabbBit) + if (flags & b2Draw::e_aabbBit) { b2Color color(0.9f, 0.3f, 0.9f); b2BroadPhase* bp = &m_contactManager.m_broadPhase; @@ -1047,24 +1191,28 @@ void b2World::DrawDebugData() for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext()) { - b2AABB aabb = bp->GetFatAABB(f->m_proxyId); - b2Vec2 vs[4]; - vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y); - vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y); - vs[2].Set(aabb.upperBound.x, aabb.upperBound.y); - vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y); + for (int32 i = 0; i < f->m_proxyCount; ++i) + { + b2FixtureProxy* proxy = f->m_proxies + i; + b2AABB aabb = bp->GetFatAABB(proxy->proxyId); + b2Vec2 vs[4]; + vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y); + vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y); + vs[2].Set(aabb.upperBound.x, aabb.upperBound.y); + vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y); - m_debugDraw->DrawPolygon(vs, 4, color); + m_debugDraw->DrawPolygon(vs, 4, color); + } } } } - if (flags & b2DebugDraw::e_centerOfMassBit) + if (flags & b2Draw::e_centerOfMassBit) { for (b2Body* b = m_bodyList; b; b = b->GetNext()) { b2Transform xf = b->GetTransform(); - xf.position = b->GetWorldCenter(); + xf.p = b->GetWorldCenter(); m_debugDraw->DrawTransform(xf); } } @@ -1074,3 +1222,18 @@ int32 b2World::GetProxyCount() const { return m_contactManager.m_broadPhase.GetProxyCount(); } + +int32 b2World::GetTreeHeight() const +{ + return m_contactManager.m_broadPhase.GetTreeHeight(); +} + +int32 b2World::GetTreeBalance() const +{ + return m_contactManager.m_broadPhase.GetTreeBalance(); +} + +float32 b2World::GetTreeQuality() const +{ + return m_contactManager.m_broadPhase.GetTreeQuality(); +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2World.h b/src/modules/physics/box2d/Box2D/Dynamics/b2World.h old mode 100644 new mode 100755 index bff14272f..381e1f1ac --- a/src/modules/physics/box2d/Box2D/Dynamics/b2World.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2World.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -24,12 +24,14 @@ #include #include #include +#include struct b2AABB; struct b2BodyDef; +struct b2Color; struct b2JointDef; -struct b2TimeStep; class b2Body; +class b2Draw; class b2Fixture; class b2Joint; @@ -63,7 +65,7 @@ public: /// Register a routine for debug drawing. The debug draw functions are called /// inside with b2World::DrawDebugData method. The debug draw object is owned /// by you and must remain in scope. - void SetDebugDraw(b2DebugDraw* debugDraw); + void SetDebugDraw(b2Draw* debugDraw); /// Create a rigid body given a definition. No reference to the definition /// is retained. @@ -94,9 +96,12 @@ public: int32 velocityIterations, int32 positionIterations); - /// Call this after you are done with time steps to clear the forces. You normally - /// call this after each call to Step, unless you are performing sub-steps. By default, - /// forces will be automatically cleared, so you don't need to call this function. + /// Manually clear the force buffer on all bodies. By default, forces are cleared automatically + /// after each call to Step. The default behavior is modified by calling SetAutoClearForces. + /// The purpose of this function is to support sub-stepping. Sub-stepping is often used to maintain + /// a fixed sized time step under a variable frame-rate. + /// When you perform sub-stepping you will disable auto clearing of forces and instead call + /// ClearForces after all sub-steps are complete in one pass of your game loop. /// @see SetAutoClearForces void ClearForces(); @@ -121,17 +126,21 @@ public: /// the next body in the world list. A NULL body indicates the end of the list. /// @return the head of the world body list. b2Body* GetBodyList(); + const b2Body* GetBodyList() const; /// Get the world joint list. With the returned joint, use b2Joint::GetNext to get /// the next joint in the world list. A NULL joint indicates the end of the list. /// @return the head of the world joint list. b2Joint* GetJointList(); + const b2Joint* GetJointList() const; /// Get the world contact list. With the returned contact, use b2Contact::GetNext to get /// the next contact in the world list. A NULL contact indicates the end of the list. /// @return the head of the world contact list. - /// @warning contacts are + /// @warning contacts are created and destroyed in the middle of a time step. + /// Use b2ContactListener to avoid missing contacts. b2Contact* GetContactList(); + const b2Contact* GetContactList() const; /// Enable/disable warm starting. For testing. void SetWarmStarting(bool flag) { m_warmStarting = flag; } @@ -139,6 +148,9 @@ public: /// Enable/disable continuous physics. For testing. void SetContinuousPhysics(bool flag) { m_continuousPhysics = flag; } + /// Enable/disable single stepped continuous physics. For testing. + void SetSubStepping(bool flag) { m_subStepping = flag; } + /// Get the number of broad-phase proxies. int32 GetProxyCount() const; @@ -151,6 +163,16 @@ public: /// Get the number of contacts (each may have 0 or more contact points). int32 GetContactCount() const; + /// Get the height of the dynamic tree. + int32 GetTreeHeight() const; + + /// Get the balance of the dynamic tree. + int32 GetTreeBalance() const; + + /// Get the quality metric of the dynamic tree. The smaller the better. + /// The minimum is 1. + float32 GetTreeQuality() const; + /// Change the global gravity vector. void SetGravity(const b2Vec2& gravity); @@ -166,6 +188,12 @@ public: /// Get the flag that controls automatic clearing of forces after each time step. bool GetAutoClearForces() const; + /// Get the contact manager for testing. + const b2ContactManager& GetContactManager() const; + + /// Get the current profile. + const b2Profile& GetProfile() const; + private: // m_flags @@ -173,16 +201,16 @@ private: { e_newFixture = 0x0001, e_locked = 0x0002, - e_clearForces = 0x0004, + e_clearForces = 0x0004 }; friend class b2Body; + friend class b2Fixture; friend class b2ContactManager; friend class b2Controller; void Solve(const b2TimeStep& step); - void SolveTOI(); - void SolveTOI(b2Body* body); + void SolveTOI(const b2TimeStep& step); void DrawJoint(b2Joint* joint); void DrawShape(b2Fixture* shape, const b2Transform& xf, const b2Color& color); @@ -203,20 +231,21 @@ private: b2Vec2 m_gravity; bool m_allowSleep; - b2Body* m_groundBody; - b2DestructionListener* m_destructionListener; - b2DebugDraw* m_debugDraw; + b2Draw* m_debugDraw; // This is used to compute the time step ratio to // support a variable time step. float32 m_inv_dt0; - // This is for debugging the solver. + // These are for debugging the solver. bool m_warmStarting; - - // This is for debugging the solver. bool m_continuousPhysics; + bool m_subStepping; + + bool m_stepComplete; + + b2Profile m_profile; }; inline b2Body* b2World::GetBodyList() @@ -224,16 +253,31 @@ inline b2Body* b2World::GetBodyList() return m_bodyList; } +inline const b2Body* b2World::GetBodyList() const +{ + return m_bodyList; +} + inline b2Joint* b2World::GetJointList() { return m_jointList; } +inline const b2Joint* b2World::GetJointList() const +{ + return m_jointList; +} + inline b2Contact* b2World::GetContactList() { return m_contactManager.m_contactList; } +inline const b2Contact* b2World::GetContactList() const +{ + return m_contactManager.m_contactList; +} + inline int32 b2World::GetBodyCount() const { return m_bodyCount; @@ -282,4 +326,14 @@ inline bool b2World::GetAutoClearForces() const return (m_flags & e_clearForces) == e_clearForces; } +inline const b2ContactManager& b2World::GetContactManager() const +{ + return m_contactManager; +} + +inline const b2Profile& b2World::GetProfile() const +{ + return m_profile; +} + #endif diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2WorldCallbacks.cpp b/src/modules/physics/box2d/Box2D/Dynamics/b2WorldCallbacks.cpp old mode 100644 new mode 100755 index bc5fefbdf..82b28cc05 --- a/src/modules/physics/box2d/Box2D/Dynamics/b2WorldCallbacks.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2WorldCallbacks.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -34,28 +34,3 @@ bool b2ContactFilter::ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB) bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0; return collide; } - -b2DebugDraw::b2DebugDraw() -{ - m_drawFlags = 0; -} - -void b2DebugDraw::SetFlags(uint32 flags) -{ - m_drawFlags = flags; -} - -uint32 b2DebugDraw::GetFlags() const -{ - return m_drawFlags; -} - -void b2DebugDraw::AppendFlags(uint32 flags) -{ - m_drawFlags |= flags; -} - -void b2DebugDraw::ClearFlags(uint32 flags) -{ - m_drawFlags &= ~flags; -} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2WorldCallbacks.h b/src/modules/physics/box2d/Box2D/Dynamics/b2WorldCallbacks.h old mode 100644 new mode 100755 index 82ee67ea0..9b937e81f --- a/src/modules/physics/box2d/Box2D/Dynamics/b2WorldCallbacks.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2WorldCallbacks.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -27,7 +27,6 @@ class b2Fixture; class b2Body; class b2Joint; class b2Contact; -struct b2ContactPoint; struct b2ContactResult; struct b2Manifold; @@ -67,6 +66,7 @@ struct b2ContactImpulse { float32 normalImpulses[b2_maxManifoldPoints]; float32 tangentImpulses[b2_maxManifoldPoints]; + int32 count; }; /// Implement this class to get contact information. You can use these results for @@ -152,66 +152,4 @@ public: const b2Vec2& normal, float32 fraction) = 0; }; -/// Color for debug drawing. Each value has the range [0,1]. -struct b2Color -{ - b2Color() {} - b2Color(float32 r, float32 g, float32 b) : r(r), g(g), b(b) {} - void Set(float32 ri, float32 gi, float32 bi) { r = ri; g = gi; b = bi; } - float32 r, g, b; -}; - -/// Implement and register this class with a b2World to provide debug drawing of physics -/// entities in your game. -class b2DebugDraw -{ -public: - b2DebugDraw(); - - virtual ~b2DebugDraw() {} - - enum - { - e_shapeBit = 0x0001, ///< draw shapes - e_jointBit = 0x0002, ///< draw joint connections - e_aabbBit = 0x0004, ///< draw axis aligned bounding boxes - e_pairBit = 0x0008, ///< draw broad-phase pairs - e_centerOfMassBit = 0x0010, ///< draw center of mass frame - }; - - /// Set the drawing flags. - void SetFlags(uint32 flags); - - /// Get the drawing flags. - uint32 GetFlags() const; - - /// Append flags to the current flags. - void AppendFlags(uint32 flags); - - /// Clear flags from the current flags. - void ClearFlags(uint32 flags); - - /// Draw a closed polygon provided in CCW order. - virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; - - /// Draw a solid closed polygon provided in CCW order. - virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; - - /// Draw a circle. - virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0; - - /// Draw a solid circle. - virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0; - - /// Draw a line segment. - virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0; - - /// Draw a transform. Choose your own length scale. - /// @param xf a transform. - virtual void DrawTransform(const b2Transform& xf) = 0; - -protected: - uint32 m_drawFlags; -}; - #endif diff --git a/src/modules/physics/box2d/Box2D/Rope/b2Rope.cpp b/src/modules/physics/box2d/Box2D/Rope/b2Rope.cpp new file mode 100755 index 000000000..97578b266 --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Rope/b2Rope.cpp @@ -0,0 +1,259 @@ +/* +* Copyright (c) 2011 Erin Catto http://box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include + +b2Rope::b2Rope() +{ + m_count = 0; + m_ps = NULL; + m_p0s = NULL; + m_vs = NULL; + m_ims = NULL; + m_Ls = NULL; + m_as = NULL; + m_gravity.SetZero(); + m_k2 = 1.0f; + m_k3 = 0.1f; +} + +b2Rope::~b2Rope() +{ + b2Free(m_ps); + b2Free(m_p0s); + b2Free(m_vs); + b2Free(m_ims); + b2Free(m_Ls); + b2Free(m_as); +} + +void b2Rope::Initialize(const b2RopeDef* def) +{ + b2Assert(def->count >= 3); + m_count = def->count; + m_ps = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); + m_p0s = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); + m_vs = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); + m_ims = (float32*)b2Alloc(m_count * sizeof(float32)); + + for (int32 i = 0; i < m_count; ++i) + { + m_ps[i] = def->vertices[i]; + m_p0s[i] = def->vertices[i]; + m_vs[i].SetZero(); + + float32 m = def->masses[i]; + if (m > 0.0f) + { + m_ims[i] = 1.0f / m; + } + else + { + m_ims[i] = 0.0f; + } + } + + int32 count2 = m_count - 1; + int32 count3 = m_count - 2; + m_Ls = (float32*)b2Alloc(count2 * sizeof(float32)); + m_as = (float32*)b2Alloc(count3 * sizeof(float32)); + + for (int32 i = 0; i < count2; ++i) + { + b2Vec2 p1 = m_ps[i]; + b2Vec2 p2 = m_ps[i+1]; + m_Ls[i] = b2Distance(p1, p2); + } + + for (int32 i = 0; i < count3; ++i) + { + b2Vec2 p1 = m_ps[i]; + b2Vec2 p2 = m_ps[i + 1]; + b2Vec2 p3 = m_ps[i + 2]; + + b2Vec2 d1 = p2 - p1; + b2Vec2 d2 = p3 - p2; + + float32 a = b2Cross(d1, d2); + float32 b = b2Dot(d1, d2); + + m_as[i] = b2Atan2(a, b); + } + + m_gravity = def->gravity; + m_damping = def->damping; + m_k2 = def->k2; + m_k3 = def->k3; +} + +void b2Rope::Step(float32 h, int32 iterations) +{ + if (h == 0.0) + { + return; + } + + float32 d = expf(- h * m_damping); + + for (int32 i = 0; i < m_count; ++i) + { + m_p0s[i] = m_ps[i]; + if (m_ims[i] > 0.0f) + { + m_vs[i] += h * m_gravity; + } + m_vs[i] *= d; + m_ps[i] += h * m_vs[i]; + + } + + for (int32 i = 0; i < iterations; ++i) + { + SolveC2(); + SolveC3(); + SolveC2(); + } + + float32 inv_h = 1.0f / h; + for (int32 i = 0; i < m_count; ++i) + { + m_vs[i] = inv_h * (m_ps[i] - m_p0s[i]); + } +} + +void b2Rope::SolveC2() +{ + int32 count2 = m_count - 1; + + for (int32 i = 0; i < count2; ++i) + { + b2Vec2 p1 = m_ps[i]; + b2Vec2 p2 = m_ps[i + 1]; + + b2Vec2 d = p2 - p1; + float32 L = d.Normalize(); + + float32 im1 = m_ims[i]; + float32 im2 = m_ims[i + 1]; + + if (im1 + im2 == 0.0f) + { + continue; + } + + float32 s1 = im1 / (im1 + im2); + float32 s2 = im2 / (im1 + im2); + + p1 -= m_k2 * s1 * (m_Ls[i] - L) * d; + p2 += m_k2 * s2 * (m_Ls[i] - L) * d; + + m_ps[i] = p1; + m_ps[i + 1] = p2; + } +} + +void b2Rope::SetAngle(float32 angle) +{ + int32 count3 = m_count - 2; + for (int32 i = 0; i < count3; ++i) + { + m_as[i] = angle; + } +} + +void b2Rope::SolveC3() +{ + int32 count3 = m_count - 2; + + for (int32 i = 0; i < count3; ++i) + { + b2Vec2 p1 = m_ps[i]; + b2Vec2 p2 = m_ps[i + 1]; + b2Vec2 p3 = m_ps[i + 2]; + + float32 m1 = m_ims[i]; + float32 m2 = m_ims[i + 1]; + float32 m3 = m_ims[i + 2]; + + b2Vec2 d1 = p2 - p1; + b2Vec2 d2 = p3 - p2; + + float32 L1sqr = d1.LengthSquared(); + float32 L2sqr = d2.LengthSquared(); + + if (L1sqr * L2sqr == 0.0f) + { + continue; + } + + float32 a = b2Cross(d1, d2); + float32 b = b2Dot(d1, d2); + + float32 angle = b2Atan2(a, b); + + b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew(); + b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew(); + + b2Vec2 J1 = -Jd1; + b2Vec2 J2 = Jd1 - Jd2; + b2Vec2 J3 = Jd2; + + float32 mass = m1 * b2Dot(J1, J1) + m2 * b2Dot(J2, J2) + m3 * b2Dot(J3, J3); + if (mass == 0.0f) + { + continue; + } + + mass = 1.0f / mass; + + float32 C = angle - m_as[i]; + + while (C > b2_pi) + { + angle -= 2 * b2_pi; + C = angle - m_as[i]; + } + + while (C < -b2_pi) + { + angle += 2.0f * b2_pi; + C = angle - m_as[i]; + } + + float32 impulse = - m_k3 * mass * C; + + p1 += (m1 * impulse) * J1; + p2 += (m2 * impulse) * J2; + p3 += (m3 * impulse) * J3; + + m_ps[i] = p1; + m_ps[i + 1] = p2; + m_ps[i + 2] = p3; + } +} + +void b2Rope::Draw(b2Draw* draw) const +{ + b2Color c(0.4f, 0.5f, 0.7f); + + for (int32 i = 0; i < m_count - 1; ++i) + { + draw->DrawSegment(m_ps[i], m_ps[i+1], c); + } +} diff --git a/src/modules/physics/box2d/Box2D/Rope/b2Rope.h b/src/modules/physics/box2d/Box2D/Rope/b2Rope.h new file mode 100755 index 000000000..bc5375ddb --- /dev/null +++ b/src/modules/physics/box2d/Box2D/Rope/b2Rope.h @@ -0,0 +1,115 @@ +/* +* Copyright (c) 2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B2_ROPE_H +#define B2_ROPE_H + +#include + +class b2Draw; + +/// +struct b2RopeDef +{ + b2RopeDef() + { + vertices = NULL; + count = 0; + masses = NULL; + gravity.SetZero(); + damping = 0.1f; + k2 = 0.9f; + k3 = 0.1f; + } + + /// + b2Vec2* vertices; + + /// + int32 count; + + /// + float32* masses; + + /// + b2Vec2 gravity; + + /// + float32 damping; + + /// Stretching stiffness + float32 k2; + + /// Bending stiffness. Values above 0.5 can make the simulation blow up. + float32 k3; +}; + +/// +class b2Rope +{ +public: + b2Rope(); + ~b2Rope(); + + /// + void Initialize(const b2RopeDef* def); + + /// + void Step(float32 timeStep, int32 iterations); + + /// + int32 GetVertexCount() const + { + return m_count; + } + + /// + const b2Vec2* GetVertices() const + { + return m_ps; + } + + /// + void Draw(b2Draw* draw) const; + + /// + void SetAngle(float32 angle); + +private: + + void SolveC2(); + void SolveC3(); + + int32 m_count; + b2Vec2* m_ps; + b2Vec2* m_p0s; + b2Vec2* m_vs; + + float32* m_ims; + + float32* m_Ls; + float32* m_as; + + b2Vec2 m_gravity; + float32 m_damping; + + float32 m_k2; + float32 m_k3; +}; + +#endif diff --git a/src/modules/physics/box2d/FrictionJoint.cpp b/src/modules/physics/box2d/FrictionJoint.cpp new file mode 100644 index 000000000..f4d818803 --- /dev/null +++ b/src/modules/physics/box2d/FrictionJoint.cpp @@ -0,0 +1,72 @@ +/** +* Copyright (c) 2006-2011 LOVE Development Team +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +**/ + +#include "FrictionJoint.h" + +#include + +// Module +#include "Body.h" +#include "World.h" + +namespace love +{ +namespace physics +{ +namespace box2d +{ + FrictionJoint::FrictionJoint(Body * body1, Body * body2, float x, float y) + : Joint(body1, body2), joint(NULL) + { + b2FrictionJointDef def; + def.Initialize(body1->body, body2->body, world->scaleDown(b2Vec2(x,y))); + joint = (b2FrictionJoint*)createJoint(&def); + } + + FrictionJoint::~FrictionJoint() + { + destroyJoint(joint); + joint = 0; + } + + void FrictionJoint::setMaxForce(float force) + { + joint->SetMaxForce(force); + } + + float FrictionJoint::getMaxForce() const + { + return joint->GetMaxForce(); + } + + void FrictionJoint::setMaxTorque(float torque) + { + joint->SetMaxTorque(torque); + } + + float FrictionJoint::getMaxTorque() const + { + return joint->GetMaxTorque(); + } + + +} // box2d +} // physics +} // love diff --git a/src/modules/physics/box2d/FrictionJoint.h b/src/modules/physics/box2d/FrictionJoint.h new file mode 100644 index 000000000..ed4599f68 --- /dev/null +++ b/src/modules/physics/box2d/FrictionJoint.h @@ -0,0 +1,78 @@ +/** +* Copyright (c) 2006-2011 LOVE Development Team +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +**/ + +#ifndef LOVE_PHYSICS_BOX2D_FRICTION_JOINT_H +#define LOVE_PHYSICS_BOX2D_FRICTION_JOINT_H + +// Module +#include "Joint.h" + +namespace love +{ +namespace physics +{ +namespace box2d +{ + /** + * A FrictionJoint applies friction to a body. + **/ + class FrictionJoint : public Joint + { + private: + + // The Box2D friction joint object. + b2FrictionJoint * joint; + + public: + + /** + * Creates a new FrictionJoint connecting body1 and body2. + **/ + FrictionJoint(Body * body1, Body * body2, float x, float y); + + virtual ~FrictionJoint(); + + /** + * Sets the maximum friction force in Newtons. + **/ + void setMaxForce(float force); + + /** + * Gets the maximum friction force in Newtons. + **/ + float getMaxForce() const; + + /** + * Sets the maximum friction torque in Newton-meters. + **/ + void setMaxTorque(float torque); + + /** + * Gets the maximum friction torque in Newton-meters. + **/ + float getMaxTorque() const; + + }; + +} // box2d +} // physics +} // love + +#endif // LOVE_PHYSICS_BOX2D_FRICTION_JOINT_H diff --git a/src/modules/physics/box2d/Joint.cpp b/src/modules/physics/box2d/Joint.cpp index 14f617489..79cf039f0 100644 --- a/src/modules/physics/box2d/Joint.cpp +++ b/src/modules/physics/box2d/Joint.cpp @@ -72,6 +72,14 @@ namespace box2d return JOINT_MOUSE; case e_gearJoint: return JOINT_GEAR; + case e_frictionJoint: + return JOINT_FRICTION; + case e_weldJoint: + return JOINT_WELD; + case e_wheelJoint: + return JOINT_WHEEL; + case e_ropeJoint: + return JOINT_ROPE; default: return JOINT_INVALID; } @@ -116,6 +124,11 @@ namespace box2d { return joint->IsActive(); } + + bool Joint::getCollideConnected() const + { + return joint->GetCollideConnected(); + } } // box2d } // physics diff --git a/src/modules/physics/box2d/Joint.h b/src/modules/physics/box2d/Joint.h index ea6fe9b09..d7e41ba58 100644 --- a/src/modules/physics/box2d/Joint.h +++ b/src/modules/physics/box2d/Joint.h @@ -100,6 +100,8 @@ namespace box2d float getReactionTorque(float dt); bool isActive() const; + + bool getCollideConnected() const; protected: diff --git a/src/modules/physics/box2d/Physics.cpp b/src/modules/physics/box2d/Physics.cpp index f76d9c9d1..40fd67f12 100644 --- a/src/modules/physics/box2d/Physics.cpp +++ b/src/modules/physics/box2d/Physics.cpp @@ -176,6 +176,11 @@ namespace box2d { return new GearJoint(joint1, joint2, ratio); } + + FrictionJoint * Physics::newFrictionJoint(Body * body1, Body * body2, float x, float y) + { + return new FrictionJoint(body1, body2, x, y); + } } // box2d } // physics diff --git a/src/modules/physics/box2d/Physics.h b/src/modules/physics/box2d/Physics.h index ed9136e0e..6f631cd97 100644 --- a/src/modules/physics/box2d/Physics.h +++ b/src/modules/physics/box2d/Physics.h @@ -36,6 +36,7 @@ #include "RevoluteJoint.h" #include "PulleyJoint.h" #include "GearJoint.h" +#include "FrictionJoint.h" namespace love { @@ -200,6 +201,13 @@ namespace box2d **/ GearJoint * newGearJoint(Joint * joint1, Joint * joint2, float ratio); + /** + * Creates a new FrictionJoint connecting body1 with body2. + * @param x Anchor along the x-axis. (World coordinates) + * @param y Anchor along the y-axis. (World coordinates) + **/ + FrictionJoint * newFrictionJoint(Body * body1, Body * body2, float x, float y); + }; // Physics diff --git a/src/modules/physics/box2d/PrismaticJoint.cpp b/src/modules/physics/box2d/PrismaticJoint.cpp index f97eba491..4556073a7 100644 --- a/src/modules/physics/box2d/PrismaticJoint.cpp +++ b/src/modules/physics/box2d/PrismaticJoint.cpp @@ -58,7 +58,7 @@ namespace box2d return world->scaleDown(joint->GetJointSpeed()); } - void PrismaticJoint::setMotorEnabled(bool motor) + void PrismaticJoint::enableMotor(bool motor) { return joint->EnableMotor(motor); } @@ -70,30 +70,30 @@ namespace box2d void PrismaticJoint::setMaxMotorForce(float force) { - joint->SetMaxMotorForce(force); + joint->SetMaxMotorForce(world->scaleDown(force)); } void PrismaticJoint::setMotorSpeed(float speed) { - joint->SetMotorSpeed(speed); + joint->SetMotorSpeed(world->scaleDown(speed)); } float PrismaticJoint::getMotorSpeed() const { - return joint->GetMotorSpeed(); + return world->scaleUp(joint->GetMotorSpeed()); } - float PrismaticJoint::getMotorForce() const + float PrismaticJoint::getMotorForce(float inv_dt) const { - return joint->GetMotorForce(); + return world->scaleUp(joint->GetMotorForce(inv_dt)); } - void PrismaticJoint::setLimitsEnabled(bool limit) + void PrismaticJoint::enableLimit(bool limit) { joint->EnableLimit(limit); } - bool PrismaticJoint::isLimitsEnabled() const + bool PrismaticJoint::isLimitEnabled() const { return joint->IsLimitEnabled(); } diff --git a/src/modules/physics/box2d/PrismaticJoint.h b/src/modules/physics/box2d/PrismaticJoint.h index b7f96ed3d..38b7787e9 100644 --- a/src/modules/physics/box2d/PrismaticJoint.h +++ b/src/modules/physics/box2d/PrismaticJoint.h @@ -87,8 +87,9 @@ namespace box2d /** * Get the current motor force, usually in N. + * @param inv_dt The inverse time step. **/ - float getMotorForce() const; + float getMotorForce(float inv_dt) const; /** * Enable/disable the joint limit. diff --git a/src/modules/physics/box2d/WeldJoint.cpp b/src/modules/physics/box2d/WeldJoint.cpp new file mode 100644 index 000000000..05cb7647f --- /dev/null +++ b/src/modules/physics/box2d/WeldJoint.cpp @@ -0,0 +1,51 @@ +/** +* Copyright (c) 2006-2011 LOVE Development Team +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +**/ + +#include "WeldJoint.h" + +#include + +// Module +#include "Body.h" +#include "World.h" + +namespace love +{ +namespace physics +{ +namespace box2d +{ + WeldJoint::WeldJoint(Body * body1, Body * body2, float x, float y) + : Joint(body1, body2), joint(NULL) + { + b2WeldJointDef def; + def.Initialize(body1->body, body2->body, world->scaleDown(b2Vec2(x,y))); + joint = (b2WeldJoint*)createJoint(&def); + } + + WeldJoint::~WeldJoint() + { + destroyJoint(joint); + joint = 0; + } + +} // box2d +} // physics +} // love diff --git a/src/modules/physics/box2d/WeldJoint.h b/src/modules/physics/box2d/WeldJoint.h new file mode 100644 index 000000000..ce38a3759 --- /dev/null +++ b/src/modules/physics/box2d/WeldJoint.h @@ -0,0 +1,58 @@ +/** +* Copyright (c) 2006-2011 LOVE Development Team +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +**/ + +#ifndef LOVE_PHYSICS_BOX2D_WELD_JOINT_H +#define LOVE_PHYSICS_BOX2D_WELD_JOINT_H + +// Module +#include "Joint.h" + +namespace love +{ +namespace physics +{ +namespace box2d +{ + /** + * A WeldJoint essentially glues two bodies together. + **/ + class WeldJoint : public Joint + { + private: + + // The Box2D weld joint object. + b2WeldJoint * joint; + + public: + + /** + * Creates a new WeldJoint connecting body1 and body2. + **/ + WeldJoint(Body * body1, Body * body2, float x, float y); + + virtual ~WeldJoint(); + + }; + +} // box2d +} // physics +} // love + +#endif // LOVE_PHYSICS_BOX2D_WELD_JOINT_H diff --git a/src/modules/physics/box2d/WheelJoint.cpp b/src/modules/physics/box2d/WheelJoint.cpp new file mode 100644 index 000000000..01f836338 --- /dev/null +++ b/src/modules/physics/box2d/WheelJoint.cpp @@ -0,0 +1,116 @@ +/** +* Copyright (c) 2006-2011 LOVE Development Team +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +**/ + +#include "WheelJoint.h" + +// Module +#include "Body.h" +#include "World.h" + +namespace love +{ +namespace physics +{ +namespace box2d +{ + WheelJoint::WheelJoint(Body * body1, Body * body2, float x, float y, float ax, float ay) + : Joint(body1, body2), joint(NULL) + { + b2WheelJointDef def; + + def.Initialize(body1->body, body2->body, world->scaleDown(b2Vec2(x,y)), b2Vec2(ax,ay)); + joint = (b2WheelJoint*)createJoint(&def); + } + + WheelJoint::~WheelJoint() + { + destroyJoint(joint); + joint = 0; + } + + float WheelJoint::getJointTranslation() const + { + return world->scaleDown(joint->GetJointTranslation()); + } + + float WheelJoint::getJointSpeed() const + { + return world->scaleDown(joint->GetJointSpeed()); + } + + void WheelJoint::enableMotor(bool motor) + { + return joint->EnableMotor(motor); + } + + bool WheelJoint::isMotorEnabled() const + { + return joint->IsMotorEnabled(); + } + + void WheelJoint::setMotorSpeed(float speed) + { + joint->SetMotorSpeed(world->scaleDown(speed)); + } + + float WheelJoint::getMotorSpeed() const + { + return world->scaleUp(joint->GetMotorSpeed()); + } + + void WheelJoint::setMaxMotorTorque(float torque) + { + joint->SetMaxMotorTorque(world->scaleDown(torque)); + } + + float WheelJoint::getMaxMotorTorque() const + { + return world->scaleUp(joint->GetMaxMotorTorque()); + } + + float WheelJoint::getMotorTorque(float inv_dt) const + { + return world->scaleUp(joint->getMotorTorque(inv_dt)); + } + + void WheelJoint::setSpringFrequencyHz(float hz) + { + joint->SetSpringFrequencyHz(world->scaleDown(hz)); + } + + float WheelJoint::getSpringFrequencyHz() const + { + return world->scaleUp(joint->GetSpringFrequencyHz()); + } + + void WheelJoint::setSpringDampingRatio(float ratio) + { + joint->SetSpringDampingRatio(world->scaleDown(ratio)); + } + + float WheelJoint::getSpringDampingRatio() const + { + return world->scaleUp(joint->GetSpringDampingRatio()); + } + + +} // box2d +} // physics +} // love diff --git a/src/modules/physics/box2d/WheelJoint.h b/src/modules/physics/box2d/WheelJoint.h new file mode 100644 index 000000000..431ca6fa7 --- /dev/null +++ b/src/modules/physics/box2d/WheelJoint.h @@ -0,0 +1,127 @@ +/** +* Copyright (c) 2006-2011 LOVE Development Team +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +**/ + +#ifndef LOVE_PHYSICS_BOX2D_WHEEL_JOINT_H +#define LOVE_PHYSICS_BOX2D_WHEEL_JOINT_H + +// Module +#include "Joint.h" + +namespace love +{ +namespace physics +{ +namespace box2d +{ + /** + * WheelJoints provide two degrees of freedom: translation + * along a defined axis and rotation in the plane. Designed + * for vehicle suspensions. + **/ + class WheelJoint : public Joint + { + private: + + // The Box2D wheel joint object. + b2WheelJoint * joint; + + public: + + /** + * Creates a new WheelJoint connecting body1 and body2. + **/ + WheelJoint(Body * body1, Body * body2, float x, float y, float ax, float ay); + + virtual ~WheelJoint(); + + /** + * Get the current joint translation, usually in meters. + **/ + float getJointTranslation() const; + + /** + * Get the current joint translation speed, usually in meters per second. + **/ + float getJointSpeed() const; + + /** + * Enable/disable the joint motor. + **/ + void enableMotor(bool motor); + + /** + * Checks whether the motor is enabled. + **/ + bool isMotorEnabled() const; + + /** + * Set the motor speed, usually in meters per second. + **/ + void setMotorSpeed(float speed); + + /** + * Get the motor speed, usually in meters per second. + **/ + float getMotorSpeed() const; + + /** + * Set the maximum motor torque, usually in N. + **/ + void setMaxMotorTorque(float torque); + + /** + * Get the maximum motor torque, usually in N. + **/ + float getMaxMotorTorque() const; + + /** + * Get the current motor torque, usually in N. + * @param inv_dt The inverse time step. + **/ + float getMotorTorque(float inv_dt) const; + + /** + * Set the spring frequency, in hertz. Setting the frequency to 0 + * disables the spring. + **/ + void setSpringFrequencyHz(float hz); + + /** + * Get the spring frequency, in hertz. + **/ + float getSpringFrequencyHz() const; + + /** + * Set the spring damping ratio. + **/ + void setSpringDampingRatio(float ratio); + + /** + * Get the spring damping ratio. + **/ + float getSpringDampingRatio() const; + + }; + +} // box2d +} // physics +} // love + +#endif // LOVE_PHYSICS_BOX2D_WHEEL_JOINT_H diff --git a/src/modules/physics/box2d/wrap_DistanceJoint.cpp b/src/modules/physics/box2d/wrap_DistanceJoint.cpp index 94eeaa28c..65739637c 100644 --- a/src/modules/physics/box2d/wrap_DistanceJoint.cpp +++ b/src/modules/physics/box2d/wrap_DistanceJoint.cpp @@ -88,7 +88,6 @@ namespace box2d { "getAnchors", w_Joint_getAnchors }, { "getReactionForce", w_Joint_getReactionForce }, { "getReactionTorque", w_Joint_getReactionTorque }, - { "setCollideConnected", w_Joint_setCollideConnected }, { "getCollideConnected", w_Joint_getCollideConnected }, { "destroy", w_Joint_destroy }, { 0, 0 } diff --git a/src/modules/physics/box2d/wrap_FrictionJoint.cpp b/src/modules/physics/box2d/wrap_FrictionJoint.cpp new file mode 100644 index 000000000..c57492ed6 --- /dev/null +++ b/src/modules/physics/box2d/wrap_FrictionJoint.cpp @@ -0,0 +1,86 @@ +/** +* Copyright (c) 2006-2011 LOVE Development Team +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +**/ + +#include "wrap_FrictionJoint.h" + +namespace love +{ +namespace physics +{ +namespace box2d +{ + FrictionJoint * luax_checkfrictionjoint(lua_State * L, int idx) + { + return luax_checktype(L, idx, "FrictionJoint", PHYSICS_FRICTION_JOINT_T); + } + + int w_FrictionJoint_setMaxForce(lua_State * L) + { + FrictionJoint * t = luax_checkfrictionjoint(L, 1); + float arg1 = (float)luaL_checknumber(L, 2); + t->setMaxForce(arg1); + return 0; + } + + int w_FrictionJoint_getMaxForce(lua_State * L) + { + FrictionJoint * t = luax_checkfrictionjoint(L, 1); + lua_pushnumber(L, t->getMaxForce()); + return 1; + } + + int w_FrictionJoint_setMaxTorque(lua_State * L) + { + FrictionJoint * t = luax_checkfrictionjoint(L, 1); + float arg1 = (float)luaL_checknumber(L, 2); + t->setMaxTorque(arg1); + return 0; + } + + int w_FrictionJoint_getMaxTorque(lua_State * L) + { + FrictionJoint * t = luax_checkfrictionjoint(L, 1); + lua_pushnumber(L, t->getMaxTorque()); + return 1; + } + + static const luaL_Reg functions[] = { + { "setMaxForce", w_FrictionJoint_setMaxForce }, + { "getMaxForce", w_FrictionJoint_getMaxForce }, + { "setMaxTorque", w_FrictionJoint_setMaxTorque }, + { "getMaxTorque", w_FrictionJoint_getMaxTorque }, + // From Joint. + { "getType", w_Joint_getType }, + { "getAnchors", w_Joint_getAnchors }, + { "getReactionForce", w_Joint_getReactionForce }, + { "getReactionTorque", w_Joint_getReactionTorque }, + { "getCollideConnected", w_Joint_getCollideConnected }, + { "destroy", w_Joint_destroy }, + { 0, 0 } + }; + + int luaopen_frictionjoint(lua_State * L) + { + return luax_register_type(L, "FrictionJoint", functions); + } + +} // box2d +} // physics +} // love diff --git a/src/modules/physics/box2d/wrap_FrictionJoint.h b/src/modules/physics/box2d/wrap_FrictionJoint.h new file mode 100644 index 000000000..e42ebd1f0 --- /dev/null +++ b/src/modules/physics/box2d/wrap_FrictionJoint.h @@ -0,0 +1,46 @@ +/** +* Copyright (c) 2006-2011 LOVE Development Team +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +**/ + +#ifndef LOVE_PHYSICS_BOX2D_WRAP_FRICTION_JOINT_H +#define LOVE_PHYSICS_BOX2D_WRAP_FRICTION_JOINT_H + +// LOVE +#include +#include "wrap_Joint.h" +#include "FrictionJoint.h" + +namespace love +{ +namespace physics +{ +namespace box2d +{ + FrictionJoint * luax_checkfrictionjoint(lua_State * L, int idx); + int w_FrictionJoint_setMaxForce(lua_State * L); + int w_FrictionJoint_getMaxForce(lua_State * L); + int w_FrictionJoint_setMaxTorque(lua_State * L); + int w_FrictionJoint_getMaxTorque(lua_State * L); + int luaopen_frictionjoint(lua_State * L); + +} // box2d +} // physics +} // love + +#endif // LOVE_PHYSICS_BOX2D_WRAP_FRICTION_JOINT_H diff --git a/src/modules/physics/box2d/wrap_GearJoint.cpp b/src/modules/physics/box2d/wrap_GearJoint.cpp index 86a212600..125a13776 100644 --- a/src/modules/physics/box2d/wrap_GearJoint.cpp +++ b/src/modules/physics/box2d/wrap_GearJoint.cpp @@ -54,7 +54,6 @@ namespace box2d { "getAnchors", w_Joint_getAnchors }, { "getReactionForce", w_Joint_getReactionForce }, { "getReactionTorque", w_Joint_getReactionTorque }, - { "setCollideConnected", w_Joint_setCollideConnected }, { "getCollideConnected", w_Joint_getCollideConnected }, { "destroy", w_Joint_destroy }, { 0, 0 } diff --git a/src/modules/physics/box2d/wrap_Joint.cpp b/src/modules/physics/box2d/wrap_Joint.cpp index 625660ae4..f71885202 100644 --- a/src/modules/physics/box2d/wrap_Joint.cpp +++ b/src/modules/physics/box2d/wrap_Joint.cpp @@ -63,14 +63,6 @@ namespace box2d return 1; } - int w_Joint_setCollideConnected(lua_State * L) - { - Joint * t = luax_checkjoint(L, 1); - bool arg1 = luax_toboolean(L, 2); - t->setCollideConnected(arg1); - return 0; - } - int w_Joint_getCollideConnected(lua_State * L) { Joint * t = luax_checkjoint(L, 1); @@ -94,7 +86,6 @@ namespace box2d { "getAnchors", w_Joint_getAnchors }, { "getReactionForce", w_Joint_getReactionForce }, { "getReactionTorque", w_Joint_getReactionTorque }, - { "setCollideConnected", w_Joint_setCollideConnected }, { "getCollideConnected", w_Joint_getCollideConnected }, { "destroy", w_Joint_destroy }, { 0, 0 } diff --git a/src/modules/physics/box2d/wrap_Joint.h b/src/modules/physics/box2d/wrap_Joint.h index 0dfdb09d3..0337c58a1 100644 --- a/src/modules/physics/box2d/wrap_Joint.h +++ b/src/modules/physics/box2d/wrap_Joint.h @@ -36,7 +36,6 @@ namespace box2d int w_Joint_getAnchors(lua_State * L); int w_Joint_getReactionForce(lua_State * L); int w_Joint_getReactionTorque(lua_State * L); - int w_Joint_setCollideConnected(lua_State * L); int w_Joint_getCollideConnected(lua_State * L); int w_Joint_destroy(lua_State * L); int luaopen_joint(lua_State * L); diff --git a/src/modules/physics/box2d/wrap_MouseJoint.cpp b/src/modules/physics/box2d/wrap_MouseJoint.cpp index 8739b35f9..e25a74aa8 100644 --- a/src/modules/physics/box2d/wrap_MouseJoint.cpp +++ b/src/modules/physics/box2d/wrap_MouseJoint.cpp @@ -106,7 +106,6 @@ namespace box2d { "getAnchors", w_Joint_getAnchors }, { "getReactionForce", w_Joint_getReactionForce }, { "getReactionTorque", w_Joint_getReactionTorque }, - { "setCollideConnected", w_Joint_setCollideConnected }, { "getCollideConnected", w_Joint_getCollideConnected }, { "destroy", w_Joint_destroy }, { 0, 0 } diff --git a/src/modules/physics/box2d/wrap_Physics.cpp b/src/modules/physics/box2d/wrap_Physics.cpp index cd5d79cf8..434b4fa61 100644 --- a/src/modules/physics/box2d/wrap_Physics.cpp +++ b/src/modules/physics/box2d/wrap_Physics.cpp @@ -204,6 +204,17 @@ namespace box2d luax_newtype(L, "GearJoint", PHYSICS_GEAR_JOINT_T, (void*)j); return 1; } + + int w_newFrictionJoint(lua_State * L) + { + Body * body1 = luax_checktype(L, 1, "Body", PHYSICS_BODY_T); + Body * body2 = luax_checktype(L, 2, "Body", PHYSICS_BODY_T); + float x = (float)luaL_checknumber(L, 3); + float y = (float)luaL_checknumber(L, 4); + FrictionJoint * j = instance->newFrictionJoint(body1, body2, x, y); + luax_newtype(L, "FrictionJoint", PHYSICS_FRICTION_JOINT_T, (void*)j); + return 1; + } // List of functions to wrap. static const luaL_Reg functions[] = { @@ -218,6 +229,7 @@ namespace box2d { "newPrismaticJoint", w_newPrismaticJoint }, { "newPulleyJoint", w_newPulleyJoint }, { "newGearJoint", w_newGearJoint }, + { "newFrictionJoint", w_newFrictionJoint }, { 0, 0 }, }; @@ -235,6 +247,7 @@ namespace box2d luaopen_revolutejoint, luaopen_pulleyjoint, luaopen_gearjoint, + luaopen_frictionjoint, 0 }; diff --git a/src/modules/physics/box2d/wrap_Physics.h b/src/modules/physics/box2d/wrap_Physics.h index 5385a6e7b..bdf1bfee3 100644 --- a/src/modules/physics/box2d/wrap_Physics.h +++ b/src/modules/physics/box2d/wrap_Physics.h @@ -37,6 +37,7 @@ #include "wrap_RevoluteJoint.h" #include "wrap_PulleyJoint.h" #include "wrap_GearJoint.h" +#include "wrap_FrictionJoint.h" namespace love { @@ -56,6 +57,7 @@ namespace box2d int w_newPrismaticJoint(lua_State * L); int w_newPulleyJoint(lua_State * L); int w_newGearJoint(lua_State * L); + int w_newFrictionJoint(lua_State * L); extern "C" LOVE_EXPORT int luaopen_love_physics(lua_State * L); } // box2d diff --git a/src/modules/physics/box2d/wrap_PrismaticJoint.cpp b/src/modules/physics/box2d/wrap_PrismaticJoint.cpp index 2981af5f2..bffd37a56 100644 --- a/src/modules/physics/box2d/wrap_PrismaticJoint.cpp +++ b/src/modules/physics/box2d/wrap_PrismaticJoint.cpp @@ -173,7 +173,6 @@ namespace box2d { "getAnchors", w_Joint_getAnchors }, { "getReactionForce", w_Joint_getReactionForce }, { "getReactionTorque", w_Joint_getReactionTorque }, - { "setCollideConnected", w_Joint_setCollideConnected }, { "getCollideConnected", w_Joint_getCollideConnected }, { "destroy", w_Joint_destroy }, { 0, 0 } diff --git a/src/modules/physics/box2d/wrap_PulleyJoint.cpp b/src/modules/physics/box2d/wrap_PulleyJoint.cpp index aa68acda3..d6a4984fb 100644 --- a/src/modules/physics/box2d/wrap_PulleyJoint.cpp +++ b/src/modules/physics/box2d/wrap_PulleyJoint.cpp @@ -69,7 +69,6 @@ namespace box2d { "getAnchors", w_Joint_getAnchors }, { "getReactionForce", w_Joint_getReactionForce }, { "getReactionTorque", w_Joint_getReactionTorque }, - { "setCollideConnected", w_Joint_setCollideConnected }, { "getCollideConnected", w_Joint_getCollideConnected }, { "destroy", w_Joint_destroy }, { 0, 0 } diff --git a/src/modules/physics/box2d/wrap_RevoluteJoint.cpp b/src/modules/physics/box2d/wrap_RevoluteJoint.cpp index 3e87b5b4e..d770e03a4 100644 --- a/src/modules/physics/box2d/wrap_RevoluteJoint.cpp +++ b/src/modules/physics/box2d/wrap_RevoluteJoint.cpp @@ -173,7 +173,6 @@ namespace box2d { "getAnchors", w_Joint_getAnchors }, { "getReactionForce", w_Joint_getReactionForce }, { "getReactionTorque", w_Joint_getReactionTorque }, - { "setCollideConnected", w_Joint_setCollideConnected }, { "getCollideConnected", w_Joint_getCollideConnected }, { "destroy", w_Joint_destroy }, { 0, 0 } diff --git a/src/modules/physics/box2d/wrap_WeldJoint.cpp b/src/modules/physics/box2d/wrap_WeldJoint.cpp new file mode 100644 index 000000000..4d0cbc16a --- /dev/null +++ b/src/modules/physics/box2d/wrap_WeldJoint.cpp @@ -0,0 +1,52 @@ +/** +* Copyright (c) 2006-2011 LOVE Development Team +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +**/ + +#include "wrap_WeldJoint.h" + +namespace love +{ +namespace physics +{ +namespace box2d +{ + WeldJoint * luax_checkweldjoint(lua_State * L, int idx) + { + return luax_checktype(L, idx, "WeldJoint", PHYSICS_WELD_JOINT_T); + } + + static const luaL_Reg functions[] = { + // From Joint. + { "getType", w_Joint_getType }, + { "getAnchors", w_Joint_getAnchors }, + { "getReactionForce", w_Joint_getReactionForce }, + { "getReactionTorque", w_Joint_getReactionTorque }, + { "getCollideConnected", w_Joint_getCollideConnected }, + { "destroy", w_Joint_destroy }, + { 0, 0 } + }; + + int luaopen_weldjoint(lua_State * L) + { + return luax_register_type(L, "WeldJoint", functions); + } + +} // box2d +} // physics +} // love diff --git a/src/modules/physics/box2d/wrap_WeldJoint.h b/src/modules/physics/box2d/wrap_WeldJoint.h new file mode 100644 index 000000000..bf875e85c --- /dev/null +++ b/src/modules/physics/box2d/wrap_WeldJoint.h @@ -0,0 +1,42 @@ +/** +* Copyright (c) 2006-2011 LOVE Development Team +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +**/ + +#ifndef LOVE_PHYSICS_BOX2D_WRAP_WELD_JOINT_H +#define LOVE_PHYSICS_BOX2D_WRAP_WELD_JOINT_H + +// LOVE +#include +#include "wrap_Joint.h" +#include "WeldJoint.h" + +namespace love +{ +namespace physics +{ +namespace box2d +{ + WeldJoint * luax_checkweldjoint(lua_State * L, int idx); + int luaopen_weldjoint(lua_State * L); + +} // box2d +} // physics +} // love + +#endif // LOVE_PHYSICS_BOX2D_WRAP_WELD_JOINT_H diff --git a/src/modules/physics/box2d/wrap_WheelJoint.cpp b/src/modules/physics/box2d/wrap_WheelJoint.cpp new file mode 100644 index 000000000..bffd37a56 --- /dev/null +++ b/src/modules/physics/box2d/wrap_WheelJoint.cpp @@ -0,0 +1,188 @@ +/** +* Copyright (c) 2006-2011 LOVE Development Team +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +**/ + +#include "wrap_PrismaticJoint.h" + +namespace love +{ +namespace physics +{ +namespace box2d +{ + PrismaticJoint * luax_checkprismaticjoint(lua_State * L, int idx) + { + return luax_checktype(L, idx, "PrismaticJoint", PHYSICS_PRISMATIC_JOINT_T); + } + + int w_PrismaticJoint_getJointTranslation(lua_State * L) + { + PrismaticJoint * t = luax_checkprismaticjoint(L, 1); + lua_pushnumber(L, t->getJointTranslation()); + return 1; + } + + int w_PrismaticJoint_getJointSpeed(lua_State * L) + { + PrismaticJoint * t = luax_checkprismaticjoint(L, 1); + lua_pushnumber(L, t->getJointSpeed()); + return 1; + } + + int w_PrismaticJoint_enableMotor(lua_State * L) + { + PrismaticJoint * t = luax_checkprismaticjoint(L, 1); + bool arg1 = luax_toboolean(L, 2); + t->enableMotor(arg1); + return 0; + } + + int w_PrismaticJoint_isMotorEnabled(lua_State * L) + { + PrismaticJoint * t = luax_checkprismaticjoint(L, 1); + luax_pushboolean(L, t->isMotorEnabled()); + return 1; + } + + int w_PrismaticJoint_setMaxMotorForce(lua_State * L) + { + PrismaticJoint * t = luax_checkprismaticjoint(L, 1); + float arg1 = (float)luaL_checknumber(L, 2); + t->setMaxMotorForce(arg1); + return 0; + } + + int w_PrismaticJoint_setMotorSpeed(lua_State * L) + { + PrismaticJoint * t = luax_checkprismaticjoint(L, 1); + float arg1 = (float)luaL_checknumber(L, 2); + t->setMotorSpeed(arg1); + return 0; + } + + int w_PrismaticJoint_getMotorSpeed(lua_State * L) + { + PrismaticJoint * t = luax_checkprismaticjoint(L, 1); + lua_pushnumber(L, t->getMotorSpeed()); + return 1; + } + + int w_PrismaticJoint_getMotorForce(lua_State * L) + { + PrismaticJoint * t = luax_checkprismaticjoint(L, 1); + lua_pushnumber(L, t->getMotorForce()); + return 1; + } + + int w_PrismaticJoint_enableLimit(lua_State * L) + { + PrismaticJoint * t = luax_checkprismaticjoint(L, 1); + bool arg1 = luax_toboolean(L, 2); + t->enableLimit(arg1); + return 0; + } + + int w_PrismaticJoint_isLimitEnabled(lua_State * L) + { + PrismaticJoint * t = luax_checkprismaticjoint(L, 1); + luax_pushboolean(L, t->isLimitEnabled()); + return 1; + } + + int w_PrismaticJoint_setUpperLimit(lua_State * L) + { + PrismaticJoint * t = luax_checkprismaticjoint(L, 1); + float arg1 = (float)luaL_checknumber(L, 2); + t->setUpperLimit(arg1); + return 0; + } + + int w_PrismaticJoint_setLowerLimit(lua_State * L) + { + PrismaticJoint * t = luax_checkprismaticjoint(L, 1); + float arg1 = (float)luaL_checknumber(L, 2); + t->setLowerLimit(arg1); + return 0; + } + + int w_PrismaticJoint_setLimits(lua_State * L) + { + PrismaticJoint * t = luax_checkprismaticjoint(L, 1); + float arg1 = (float)luaL_checknumber(L, 2); + float arg2 = (float)luaL_checknumber(L, 3); + t->setLimits(arg1, arg2); + return 0; + } + + int w_PrismaticJoint_getLowerLimit(lua_State * L) + { + PrismaticJoint * t = luax_checkprismaticjoint(L, 1); + lua_pushnumber(L, t->getLowerLimit()); + return 1; + } + + int w_PrismaticJoint_getUpperLimit(lua_State * L) + { + PrismaticJoint * t = luax_checkprismaticjoint(L, 1); + lua_pushnumber(L, t->getUpperLimit()); + return 1; + } + + int w_PrismaticJoint_getLimits(lua_State * L) + { + PrismaticJoint * t = luax_checkprismaticjoint(L, 1); + lua_remove(L, 1); + return t->getLimits(L); + } + + static const luaL_Reg functions[] = { + { "getJointTranslation", w_PrismaticJoint_getJointTranslation }, + { "getJointSpeed", w_PrismaticJoint_getJointSpeed }, + { "enableMotor", w_PrismaticJoint_enableMotor }, + { "isMotorEnabled", w_PrismaticJoint_isMotorEnabled }, + { "setMaxMotorForce", w_PrismaticJoint_setMaxMotorForce }, + { "setMotorSpeed", w_PrismaticJoint_setMotorSpeed }, + { "getMotorSpeed", w_PrismaticJoint_getMotorSpeed }, + { "getMotorForce", w_PrismaticJoint_getMotorForce }, + { "enableLimit", w_PrismaticJoint_enableLimit }, + { "isLimitEnabled", w_PrismaticJoint_isLimitEnabled }, + { "setUpperLimit", w_PrismaticJoint_setUpperLimit }, + { "setLowerLimit", w_PrismaticJoint_setLowerLimit }, + { "setLimits", w_PrismaticJoint_setLimits }, + { "getLowerLimit", w_PrismaticJoint_getLowerLimit }, + { "getUpperLimit", w_PrismaticJoint_getUpperLimit }, + { "getLimits", w_PrismaticJoint_getLimits }, + // From Joint. + { "getType", w_Joint_getType }, + { "getAnchors", w_Joint_getAnchors }, + { "getReactionForce", w_Joint_getReactionForce }, + { "getReactionTorque", w_Joint_getReactionTorque }, + { "getCollideConnected", w_Joint_getCollideConnected }, + { "destroy", w_Joint_destroy }, + { 0, 0 } + }; + + int luaopen_prismaticjoint(lua_State * L) + { + return luax_register_type(L, "PrismaticJoint", functions); + } + +} // box2d +} // physics +} // love diff --git a/src/modules/physics/box2d/wrap_WheelJoint.h b/src/modules/physics/box2d/wrap_WheelJoint.h new file mode 100644 index 000000000..203edaea7 --- /dev/null +++ b/src/modules/physics/box2d/wrap_WheelJoint.h @@ -0,0 +1,58 @@ +/** +* Copyright (c) 2006-2011 LOVE Development Team +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +**/ + +#ifndef LOVE_PHYSICS_BOX2D_WRAP_PRISMATIC_JOINT_H +#define LOVE_PHYSICS_BOX2D_WRAP_PRISMATIC_JOINT_H + +// LOVE +#include +#include "wrap_Joint.h" +#include "PrismaticJoint.h" + +namespace love +{ +namespace physics +{ +namespace box2d +{ + PrismaticJoint * luax_checkprismaticjoint(lua_State * L, int idx); + int w_PrismaticJoint_getJointTranslation(lua_State * L); + int w_PrismaticJoint_getJointSpeed(lua_State * L); + int w_PrismaticJoint_enableMotor(lua_State * L); + int w_PrismaticJoint_isMotorEnabled(lua_State * L); + int w_PrismaticJoint_setMaxMotorForce(lua_State * L); + int w_PrismaticJoint_setMotorSpeed(lua_State * L); + int w_PrismaticJoint_getMotorSpeed(lua_State * L); + int w_PrismaticJoint_getMotorForce(lua_State * L); + int w_PrismaticJoint_enableLimit(lua_State * L); + int w_PrismaticJoint_isLimitEnabled(lua_State * L); + int w_PrismaticJoint_setUpperLimit(lua_State * L); + int w_PrismaticJoint_setLowerLimit(lua_State * L); + int w_PrismaticJoint_setLimits(lua_State * L); + int w_PrismaticJoint_getLowerLimit(lua_State * L); + int w_PrismaticJoint_getUpperLimit(lua_State * L); + int w_PrismaticJoint_getLimits(lua_State * L); + int luaopen_prismaticjoint(lua_State * L); + +} // box2d +} // physics +} // love + +#endif // LOVE_PHYSICS_BOX2D_WRAP_PRISMATIC_JOINT_H