From 146e343b49af2ade042dc416b983ef1f06cf712b Mon Sep 17 00:00:00 2001 From: chai Date: Thu, 20 Sep 2018 20:53:55 +0800 Subject: *update --- Box2d/Assets/Program/Box2d/Box2DXDebug.cs | 12 - .../Box2d/Collision/Collision.CollideCircle.cs | 155 ++-- .../Box2d/Collision/Collision.CollideEdge.cs | 100 --- .../Box2d/Collision/Collision.CollideEdge.cs.meta | 11 - .../Box2d/Collision/Collision.CollidePoly.cs | 142 ++- .../Program/Box2d/Collision/Collision.Distance.cs | 808 ++++++----------- .../Box2d/Collision/Collision.TimeOfImpact.cs | 358 ++------ Box2d/Assets/Program/Box2d/Collision/Collision.cs | 409 ++------- .../Assets/Program/Box2d/Collision/PairManager.cs | 21 +- .../Program/Box2d/Collision/Shapes/CircleShape.cs | 132 +-- .../Program/Box2d/Collision/Shapes/EdgeShape.cs | 277 ------ .../Box2d/Collision/Shapes/EdgeShape.cs.meta | 11 - .../Program/Box2d/Collision/Shapes/PolygonShape.cs | 619 +++++-------- .../Assets/Program/Box2d/Collision/Shapes/Shape.cs | 334 ++++++- Box2d/Assets/Program/Box2d/Collision/TODO.txt | 1 - Box2d/Assets/Program/Box2d/Collision/TODO.txt.meta | 7 - Box2d/Assets/Program/Box2d/Common/Mat22.cs | 2 +- Box2d/Assets/Program/Box2d/Common/Math.cs | 9 +- Box2d/Assets/Program/Box2d/Common/Settings.cs | 25 +- Box2d/Assets/Program/Box2d/Common/Sweep.cs | 25 +- Box2d/Assets/Program/Box2d/Common/Vec2.cs | 38 +- Box2d/Assets/Program/Box2d/Common/XForm.cs | 13 - Box2d/Assets/Program/Box2d/Dynamics/Body.cs | 389 ++++----- .../Program/Box2d/Dynamics/ContactManager.cs | 144 +-- .../Box2d/Dynamics/Contacts/CircleContact.cs | 108 ++- .../Program/Box2d/Dynamics/Contacts/Contact.cs | 366 ++++---- .../Box2d/Dynamics/Contacts/ContactSolver.cs | 971 ++++++++++----------- .../Dynamics/Contacts/EdgeAndCircleContact.cs | 55 -- .../Dynamics/Contacts/EdgeAndCircleContact.cs.meta | 11 - .../Program/Box2d/Dynamics/Contacts/NullContact.cs | 16 +- .../Dynamics/Contacts/PolyAndCircleContact.cs | 142 ++- .../Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs | 52 -- .../Dynamics/Contacts/PolyAndEdgeContact.cs.meta | 11 - .../Program/Box2d/Dynamics/Contacts/PolyContact.cs | 142 ++- .../Assets/Program/Box2d/Dynamics/Controllers.meta | 8 - .../Dynamics/Controllers/BuoyancyController.cs | 156 ---- .../Controllers/BuoyancyController.cs.meta | 11 - .../Controllers/ConstantAccelController.cs | 56 -- .../Controllers/ConstantAccelController.cs.meta | 11 - .../Controllers/ConstantForceController.cs | 57 -- .../Controllers/ConstantForceController.cs.meta | 11 - .../Box2d/Dynamics/Controllers/Controller.cs | 174 ---- .../Box2d/Dynamics/Controllers/Controller.cs.meta | 11 - .../Dynamics/Controllers/GravityController.cs | 96 -- .../Dynamics/Controllers/GravityController.cs.meta | 11 - .../Controllers/TensorDampingController.cs | 89 -- .../Controllers/TensorDampingController.cs.meta | 11 - Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs | 478 ---------- .../Assets/Program/Box2d/Dynamics/Fixture.cs.meta | 11 - Box2d/Assets/Program/Box2d/Dynamics/Island.cs | 166 ++-- .../Program/Box2d/Dynamics/Joints/DistanceJoint.cs | 19 +- .../Program/Box2d/Dynamics/Joints/GearJoint.cs | 64 +- .../Assets/Program/Box2d/Dynamics/Joints/Joint.cs | 41 +- .../Program/Box2d/Dynamics/Joints/LineJoint.cs | 711 +-------------- .../Program/Box2d/Dynamics/Joints/MouseJoint.cs | 87 +- .../Box2d/Dynamics/Joints/PrismaticJoint.cs | 590 ++++++------- .../Program/Box2d/Dynamics/Joints/PulleyJoint.cs | 101 ++- .../Program/Box2d/Dynamics/Joints/RevoluteJoint.cs | 388 ++++---- Box2d/Assets/Program/Box2d/Dynamics/World.cs | 371 ++++---- .../Program/Box2d/Dynamics/WorldCallbacks.cs | 107 ++- Box2d/Assets/Scenes/TestScene.unity | 140 +-- Box2d/Assets/smcs.rsp | 1 - Box2d/Assets/smcs.rsp.meta | 7 - 63 files changed, 3450 insertions(+), 6450 deletions(-) delete mode 100644 Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs delete mode 100644 Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs.meta delete mode 100644 Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs delete mode 100644 Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs.meta delete mode 100644 Box2d/Assets/Program/Box2d/Collision/TODO.txt delete mode 100644 Box2d/Assets/Program/Box2d/Collision/TODO.txt.meta delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs.meta delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs.meta delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Controllers.meta delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs.meta delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs.meta delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs.meta delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs.meta delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs.meta delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs.meta delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs delete mode 100644 Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs.meta delete mode 100644 Box2d/Assets/smcs.rsp delete mode 100644 Box2d/Assets/smcs.rsp.meta diff --git a/Box2d/Assets/Program/Box2d/Box2DXDebug.cs b/Box2d/Assets/Program/Box2d/Box2DXDebug.cs index 49013a6..50fa35c 100644 --- a/Box2d/Assets/Program/Box2d/Box2DXDebug.cs +++ b/Box2d/Assets/Program/Box2d/Box2DXDebug.cs @@ -31,30 +31,18 @@ namespace Box2DX [Conditional("DEBUG")] public static void Assert(bool condition) { - if (!condition) - { - condition = condition; - } Debug.Assert(condition); } [Conditional("DEBUG")] public static void Assert(bool condition, string message) { - if (!condition) - { - condition = condition; - } Debug.Assert(condition, message); } [Conditional("DEBUG")] public static void Assert(bool condition, string message, string detailMessage) { - if (!condition) - { - condition = condition; - } Debug.Assert(condition, message, detailMessage); } diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.CollideCircle.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.CollideCircle.cs index fa10a9e..973ca6c 100644 --- a/Box2d/Assets/Program/Box2d/Collision/Collision.CollideCircle.cs +++ b/Box2d/Assets/Program/Box2d/Collision/Collision.CollideCircle.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -19,6 +19,10 @@ 3. This notice may not be removed or altered from any source distribution. */ +using System; +using System.Collections.Generic; +using System.Text; + using Box2DX.Common; namespace Box2DX.Collision @@ -30,24 +34,45 @@ namespace Box2DX.Collision { manifold.PointCount = 0; - Vec2 p1 = Common.Math.Mul(xf1, circle1._position); - Vec2 p2 = Common.Math.Mul(xf2, circle2._position); + Vec2 p1 = Common.Math.Mul(xf1, circle1.GetLocalPosition()); + Vec2 p2 = Common.Math.Mul(xf2, circle2.GetLocalPosition()); Vec2 d = p2 - p1; float distSqr = Vec2.Dot(d, d); - float radius = circle1._radius + circle2._radius; - if (distSqr > radius * radius) + float r1 = circle1.GetRadius(); + float r2 = circle2.GetRadius(); + float radiusSum = r1 + r2; + if (distSqr > radiusSum * radiusSum) { return; } - manifold.Type = ManifoldType.Circles; - manifold.LocalPoint = circle1._position; - manifold.LocalPlaneNormal.SetZero(); - manifold.PointCount = 1; + float separation; + if (distSqr < Common.Settings.FLT_EPSILON) + { + separation = -radiusSum; + manifold.Normal.Set(0.0f, 1.0f); + } + else + { + float dist = Common.Math.Sqrt(distSqr); + separation = dist - radiusSum; + float a = 1.0f / dist; + manifold.Normal.X = a * d.X; + manifold.Normal.Y = a * d.Y; + } - manifold.Points[0].LocalPoint = circle2._position; + manifold.PointCount = 1; manifold.Points[0].ID.Key = 0; + manifold.Points[0].Separation = separation; + + p1 += r1 * manifold.Normal; + p2 -= r2 * manifold.Normal; + + Vec2 p = 0.5f * (p1 + p2); + + manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, p); + manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, p); } public static void CollidePolygonAndCircle(ref Manifold manifold, @@ -56,16 +81,16 @@ namespace Box2DX.Collision manifold.PointCount = 0; // Compute circle position in the frame of the polygon. - Vec2 c = Common.Math.Mul(xf2, circle._position); + Vec2 c = Common.Math.Mul(xf2, circle.GetLocalPosition()); Vec2 cLocal = Common.Math.MulT(xf1, c); // Find the min separating edge. int normalIndex = 0; float separation = -Settings.FLT_MAX; - float radius = polygon._radius + circle._radius; - int vertexCount = polygon._vertexCount; - Vec2[] vertices = polygon._vertices; - Vec2[] normals = polygon._normals; + float radius = circle.GetRadius(); + int vertexCount = polygon.VertexCount; + Vec2[] vertices = polygon.GetVertices(); + Vec2[] normals = polygon.Normals; for (int i = 0; i < vertexCount; ++i) { @@ -83,73 +108,67 @@ namespace Box2DX.Collision } } - // Vertices that subtend the incident face. - int vertIndex1 = normalIndex; - int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; - Vec2 v1 = vertices[vertIndex1]; - Vec2 v2 = vertices[vertIndex2]; - // If the center is inside the polygon ... if (separation < Common.Settings.FLT_EPSILON) { manifold.PointCount = 1; - manifold.Type = ManifoldType.FaceA; - manifold.LocalPlaneNormal = normals[normalIndex]; - manifold.LocalPoint = 0.5f * (v1 + v2); - manifold.Points[0].LocalPoint = circle._position; - manifold.Points[0].ID.Key = 0; + manifold.Normal = Common.Math.Mul(xf1.R, normals[normalIndex]); + manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex; + manifold.Points[0].ID.Features.IncidentVertex = Collision.NullFeature; + manifold.Points[0].ID.Features.ReferenceEdge = 0; + manifold.Points[0].ID.Features.Flip = 0; + Vec2 position = c - radius * manifold.Normal; + manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, position); + manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, position); + manifold.Points[0].Separation = separation - radius; return; } - // Compute barycentric coordinates - float u1 = Vec2.Dot(cLocal - v1, v2 - v1); - float u2 = Vec2.Dot(cLocal - v2, v1 - v2); - if (u1 <= 0.0f) - { - if (Vec2.DistanceSquared(cLocal, v1) > radius * radius) - { - return; - } + // Project the circle center onto the edge segment. + int vertIndex1 = normalIndex; + int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; + Vec2 e = vertices[vertIndex2] - vertices[vertIndex1]; - manifold.PointCount = 1; - manifold.Type = ManifoldType.FaceA; - manifold.LocalPlaneNormal = cLocal - v1; - manifold.LocalPlaneNormal.Normalize(); - manifold.LocalPoint = v1; - manifold.Points[0].LocalPoint = circle._position; - manifold.Points[0].ID.Key = 0; + float length = e.Normalize(); + Box2DXDebug.Assert(length > Settings.FLT_EPSILON); + + // Project the center onto the edge. + float u = Vec2.Dot(cLocal - vertices[vertIndex1], e); + Vec2 p; + if (u <= 0.0f) + { + p = vertices[vertIndex1]; + manifold.Points[0].ID.Features.IncidentEdge = Collision.NullFeature; + manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex1; } - else if (u2 <= 0.0f) + else if (u >= length) { - if (Vec2.DistanceSquared(cLocal, v2) > radius * radius) - { - return; - } - - manifold.PointCount = 1; - manifold.Type = ManifoldType.FaceA; - manifold.LocalPlaneNormal = cLocal - v2; - manifold.LocalPlaneNormal.Normalize(); - manifold.LocalPoint = v2; - manifold.Points[0].LocalPoint = circle._position; - manifold.Points[0].ID.Key = 0; + p = vertices[vertIndex2]; + manifold.Points[0].ID.Features.IncidentEdge = Collision.NullFeature; + manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex2; } else { - Vec2 faceCenter = 0.5f * (v1 + v2); - float separation_ = Vec2.Dot(cLocal - faceCenter, normals[vertIndex1]); - if (separation_ > radius) - { - return; - } + p = vertices[vertIndex1] + u * e; + manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex; + manifold.Points[0].ID.Features.IncidentVertex = Collision.NullFeature; + } - manifold.PointCount = 1; - manifold.Type = ManifoldType.FaceA; - manifold.LocalPlaneNormal = normals[vertIndex1]; - manifold.LocalPoint = faceCenter; - manifold.Points[0].LocalPoint = circle._position; - manifold.Points[0].ID.Key = 0; + Vec2 d = cLocal - p; + float dist = d.Normalize(); + if (dist > radius) + { + return; } + + manifold.PointCount = 1; + manifold.Normal = Common.Math.Mul(xf1.R, d); + Vec2 position_ = c - radius * manifold.Normal; + manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, position_); + manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, position_); + manifold.Points[0].Separation = dist - radius; + manifold.Points[0].ID.Features.ReferenceEdge = 0; + manifold.Points[0].ID.Features.Flip = 0; } } } \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs deleted file mode 100644 index 8ea3821..0000000 --- a/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs +++ /dev/null @@ -1,100 +0,0 @@ -/* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 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. -*/ - -using Box2DX.Common; - -namespace Box2DX.Collision -{ - public partial class Collision - { - // This implements 2-sided edge vs circle collision. - public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edge, XForm transformA, CircleShape circle, XForm transformB) - { - manifold.PointCount = 0; - Vec2 cLocal = Common.Math.MulT(transformA, Common.Math.Mul(transformB, circle._position)); - Vec2 normal = edge._normal; - Vec2 v1 = edge._v1; - Vec2 v2 = edge._v2; - float radius = edge._radius + circle._radius; - - // Barycentric coordinates - float u1 = Vec2.Dot(cLocal - v1, v2 - v1); - float u2 = Vec2.Dot(cLocal - v2, v1 - v2); - - if (u1 <= 0.0f) - { - // Behind v1 - if (Vec2.DistanceSquared(cLocal, v1) > radius * radius) - { - return; - } - - manifold.PointCount = 1; - manifold.Type = ManifoldType.FaceA; - manifold.LocalPlaneNormal = cLocal - v1; - manifold.LocalPlaneNormal.Normalize(); - manifold.LocalPoint = v1; - manifold.Points[0].LocalPoint = circle._position; - manifold.Points[0].ID.Key = 0; - } - else if (u2 <= 0.0f) - { - // Ahead of v2 - if (Vec2.DistanceSquared(cLocal, v2) > radius * radius) - { - return; - } - - manifold.PointCount = 1; - manifold.Type = ManifoldType.FaceA; - manifold.LocalPlaneNormal = cLocal - v2; - manifold.LocalPlaneNormal.Normalize(); - manifold.LocalPoint = v2; - manifold.Points[0].LocalPoint = circle._position; - manifold.Points[0].ID.Key = 0; - } - else - { - float separation = Vec2.Dot(cLocal - v1, normal); - if (separation < -radius || radius < separation) - { - return; - } - - manifold.PointCount = 1; - manifold.Type = ManifoldType.FaceA; - manifold.LocalPlaneNormal = separation < 0.0f ? -normal : normal; - manifold.LocalPoint = 0.5f * (v1 + v2); - manifold.Points[0].LocalPoint = circle._position; - manifold.Points[0].ID.Key = 0; - } - } - - // Polygon versus 2-sided edge. - public static void CollidePolyAndEdge(ref Manifold manifold, PolygonShape polygon, XForm transformA, EdgeShape edge, XForm transformB) - { - PolygonShape polygonB = new PolygonShape(); - polygonB.SetAsEdge(edge._v1, edge._v2); - - CollidePolygons(ref manifold, polygon, transformA, polygonB, transformB); - } - } -} \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs.meta b/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs.meta deleted file mode 100644 index b71db55..0000000 --- a/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs.meta +++ /dev/null @@ -1,11 +0,0 @@ -fileFormatVersion: 2 -guid: b683cd8505628a247bde0d512af54021 -MonoImporter: - externalObjects: {} - serializedVersion: 2 - defaultReferences: [] - executionOrder: 0 - icon: {instanceID: 0} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.CollidePoly.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.CollidePoly.cs index 65c48e4..87f64e6 100644 --- a/Box2d/Assets/Program/Box2d/Collision/Collision.CollidePoly.cs +++ b/Box2d/Assets/Program/Box2d/Collision/Collision.CollidePoly.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -19,23 +19,78 @@ 3. This notice may not be removed or altered from any source distribution. */ +using System; +using System.Collections.Generic; +using System.Text; + using Box2DX.Common; namespace Box2DX.Collision { public partial class Collision { + public struct ClipVertex + { + public Vec2 V; + public ContactID ID; + } + + public static int ClipSegmentToLine(out ClipVertex[] vOut, ClipVertex[] vIn, + Vec2 normal, float offset) + { + if (vIn.Length != 2) + Box2DXDebug.ThrowBox2DXException("vIn should contain 2 element, but contains " + vIn.Length.ToString()); + vOut = new ClipVertex[2]; + + // Start with no output points + int numOut = 0; + + // Calculate the distance of end points to the line + float distance0 = Vec2.Dot(normal, vIn[0].V) - offset; + float distance1 = Vec2.Dot(normal, vIn[1].V) - offset; + + // If the points are behind the plane + if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; + if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; + + // If the points are on different sides of the plane + if (distance0 * distance1 < 0.0f) + { + // Find intersection point of edge and plane + float 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; + } + ++numOut; + } + + return numOut; + } + /// /// Find the separation between poly1 and poly2 for a give edge normal on poly1. /// - public static float EdgeSeparation(PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2) + /// + /// + /// + /// + /// + /// + public static float EdgeSeparation(PolygonShape poly1, XForm xf1, int edge1, + PolygonShape poly2, XForm xf2) { - int count1 = poly1._vertexCount; - Vec2[] vertices1 = poly1._vertices; - Vec2[] normals1 = poly1._normals; + int count1 = poly1.VertexCount; + Vec2[] vertices1 = poly1.GetVertices(); + Vec2[] normals1 = poly1.Normals; - int count2 = poly2._vertexCount; - Vec2[] vertices2 = poly2._vertices; + int count2 = poly2.VertexCount; + Vec2[] vertices2 = poly2.GetVertices(); Box2DXDebug.Assert(0 <= edge1 && edge1 < count1); @@ -65,13 +120,20 @@ namespace Box2DX.Collision /// /// Find the max separation between poly1 and poly2 using edge normals from poly1. /// - public static float FindMaxSeparation(ref int edgeIndex, PolygonShape poly1, XForm xf1, PolygonShape poly2, XForm xf2) + /// + /// + /// + /// + /// + /// + public static float FindMaxSeparation(ref int edgeIndex, + PolygonShape poly1, XForm xf1, PolygonShape poly2, XForm xf2) { - int count1 = poly1._vertexCount; - Vec2[] normals1 = poly1._normals; + int count1 = poly1.VertexCount; + Vec2[] normals1 = poly1.Normals; // Vector pointing from the centroid of poly1 to the centroid of poly2. - Vec2 d = Common.Math.Mul(xf2, poly2._centroid) - Common.Math.Mul(xf1, poly1._centroid); + Vec2 d = Common.Math.Mul(xf2, poly2.GetCentroid()) - Common.Math.Mul(xf1, poly1.GetCentroid()); Vec2 dLocal1 = Common.Math.MulT(xf1.R, d); // Find edge normal on poly1 that has the largest projection onto d. @@ -89,14 +151,26 @@ namespace Box2DX.Collision // Get the separation for the edge normal. float s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2); + if (s > 0.0f) + { + return s; + } // Check the separation for the previous edge normal. int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; float sPrev = Collision.EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2); + if (sPrev > 0.0f) + { + return sPrev; + } // Check the separation for the next edge normal. int nextEdge = edge + 1 < count1 ? edge + 1 : 0; float sNext = Collision.EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2); + if (sNext > 0.0f) + { + return sNext; + } // Find the best edge and the search direction. int bestEdge; @@ -129,6 +203,10 @@ namespace Box2DX.Collision edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0; s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2); + if (s > 0.0f) + { + return s; + } if (s > bestSeparation) { @@ -148,12 +226,12 @@ namespace Box2DX.Collision public static void FindIncidentEdge(out ClipVertex[] c, PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2) { - int count1 = poly1._vertexCount; - Vec2[] normals1 = poly1._normals; + int count1 = poly1.VertexCount; + Vec2[] normals1 = poly1.Normals; - int count2 = poly2._vertexCount; - Vec2[] vertices2 = poly2._vertices; - Vec2[] normals2 = poly2._normals; + int count2 = poly2.VertexCount; + Vec2[] vertices2 = poly2.GetVertices(); + Vec2[] normals2 = poly2.Normals; Box2DXDebug.Assert(0 <= edge1 && edge1 < count1); @@ -200,16 +278,15 @@ namespace Box2DX.Collision PolygonShape polyA, XForm xfA, PolygonShape polyB, XForm xfB) { manifold.PointCount = 0; - float totalRadius = polyA._radius + polyB._radius; int edgeA = 0; float separationA = Collision.FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB); - if (separationA > totalRadius) + if (separationA > 0.0f) return; int edgeB = 0; float separationB = Collision.FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA); - if (separationB > totalRadius) + if (separationB > 0.0f) return; PolygonShape poly1; // reference poly @@ -217,9 +294,10 @@ namespace Box2DX.Collision XForm xf1, xf2; int edge1; // reference edge byte flip; - const float k_relativeTol = 0.98f; - const float k_absoluteTol = 0.001f; + float k_relativeTol = 0.98f; + float k_absoluteTol = 0.001f; + // TODO_ERIN use "radius" of poly for absolute tolerance. if (separationB > k_relativeTol * separationA + k_absoluteTol) { poly1 = polyB; @@ -227,7 +305,6 @@ namespace Box2DX.Collision xf1 = xfB; xf2 = xfA; edge1 = edgeB; - manifold.Type = ManifoldType.FaceB; flip = 1; } else @@ -237,25 +314,19 @@ namespace Box2DX.Collision xf1 = xfA; xf2 = xfB; edge1 = edgeA; - manifold.Type = ManifoldType.FaceA; flip = 0; } ClipVertex[] incidentEdge; Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2); - int count1 = poly1._vertexCount; - Vec2[] vertices1 = poly1._vertices; + int count1 = poly1.VertexCount; + Vec2[] vertices1 = poly1.GetVertices(); Vec2 v11 = vertices1[edge1]; Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0]; Vec2 dv = v12 - v11; - - Vec2 localNormal = Vec2.Cross(dv, 1.0f); - localNormal.Normalize(); - Vec2 planePoint = 0.5f * (v11 + v12); - Vec2 sideNormal = Common.Math.Mul(xf1.R, v12 - v11); sideNormal.Normalize(); Vec2 frontNormal = Vec2.Cross(sideNormal, 1.0f); @@ -285,18 +356,19 @@ namespace Box2DX.Collision return; // Now clipPoints2 contains the clipped points. - manifold.LocalPlaneNormal = localNormal; - manifold.LocalPoint = planePoint; + manifold.Normal = flip!=0 ? -frontNormal : frontNormal; int pointCount = 0; for (int i = 0; i < Settings.MaxManifoldPoints; ++i) { float separation = Vec2.Dot(frontNormal, clipPoints2[i].V) - frontOffset; - if (separation <= totalRadius) + if (separation <= 0.0f) { ManifoldPoint cp = manifold.Points[pointCount]; - cp.LocalPoint = Common.Math.MulT(xf2, clipPoints2[i].V); + cp.Separation = separation; + cp.LocalPoint1 = Box2DX.Common.Math.MulT(xfA, clipPoints2[i].V); + cp.LocalPoint2 = Box2DX.Common.Math.MulT(xfB, clipPoints2[i].V); cp.ID = clipPoints2[i].ID; cp.ID.Features.Flip = flip; ++pointCount; diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.Distance.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.Distance.cs index d6abeb6..71b6d49 100644 --- a/Box2d/Assets/Program/Box2d/Collision/Collision.Distance.cs +++ b/Box2d/Assets/Program/Box2d/Collision/Collision.Distance.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -19,612 +19,370 @@ 3. This notice may not be removed or altered from any source distribution. */ -#define DEBUG - using System; +using System.Collections.Generic; +using System.Text; + using Box2DX.Common; namespace Box2DX.Collision { - /// - /// Used to warm start Distance. - /// Set count to zero on first call. - /// - public struct SimplexCache + public partial class Collision { - /// - /// Length or area. - /// - public Single Metric; - public UInt16 Count; - /// - /// Vertices on shape A. - /// - //public Byte[/*3*/] IndexA; - public IndexArray IndexA; - /// - /// Vertices on shape B. - /// - //public Byte[/*3*/] IndexB; - public IndexArray IndexB; - - //public SimplexCache(byte init) - //{ - // Metric = 0; - // Count = 0; - // IndexA = new Byte[3]; - // IndexB = new Byte[3]; - //} - } + public static int GJKIterations = 0; - public struct IndexArray - { - private Byte I0, I1, I2; + // GJK using Voronoi regions (Christer Ericson) and region selection + // optimizations (Casey Muratori). - public Byte this[int index] + // The origin is either in the region of points[1] or in the edge region. The origin is + // not in region of points[0] because that is the old point. + public static int ProcessTwo(out Vec2 x1, out Vec2 x2, ref Vec2[] p1s, ref Vec2[] p2s, + ref Vec2[] points) { - get + // If in point[1] region + Vec2 r = -points[1]; + Vec2 d = points[0] - points[1]; + float length = d.Normalize(); + float lambda = Vec2.Dot(r, d); + if (lambda <= 0.0f || length < Common.Settings.FLT_EPSILON) { -#if DEBUG - Box2DXDebug.Assert(index >= 0 && index < 3); -#endif - if (index == 0) return I0; - else if (index == 1) return I1; - else return I2; + // The simplex is reduced to a point. + x1 = p1s[1]; + x2 = p2s[1]; + p1s[0] = p1s[1]; + p2s[0] = p2s[1]; + points[0] = points[1]; + return 1; } - set - { -#if DEBUG - Box2DXDebug.Assert(index >= 0 && index < 3); -#endif - if (index == 0) I0 = value; - else if (index == 1) I1 = value; - else I2 = value; - } - } - } - - /// - /// Input for Distance. - /// You have to option to use the shape radii - /// in the computation. - /// - public struct DistanceInput - { - public XForm TransformA; - public XForm TransformB; - public bool UseRadii; - } - - /// - /// Output for Distance. - /// - public struct DistanceOutput - { - /// - /// Closest point on shapeA. - /// - public Vec2 PointA; - /// - /// Closest point on shapeB. - /// - public Vec2 PointB; - public float Distance; - /// - /// Number of GJK iterations used. - /// - public int Iterations; - } - - // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. - - internal struct SimplexVertex - { - internal Vec2 wA; // support point in shapeA - internal Vec2 wB; // support point in shapeB - internal Vec2 w; // wB - wA - internal float a; // barycentric coordinate for closest point - internal int indexA; // wA index - internal int indexB; // wB index - } - - internal struct Simplex - { - internal SimplexVertex _v1, _v2, _v3; - internal int _count; - - internal unsafe void ReadCache(SimplexCache* cache, Shape shapeA, XForm transformA, Shape shapeB, XForm transformB) - { - Box2DXDebug.Assert(0 <= cache->Count && cache->Count <= 3); - - // Copy data from cache. - _count = cache->Count; - SimplexVertex** vertices = stackalloc SimplexVertex*[3]; - fixed (SimplexVertex* v1Ptr = &_v1, v2Ptr = &_v2, v3Ptr = &_v3) - { - vertices[0] = v1Ptr; - vertices[1] = v2Ptr; - vertices[2] = v3Ptr; - for (int i = 0; i < _count; ++i) - { - SimplexVertex* v = vertices[i]; - v->indexA = cache->IndexA[i]; - v->indexB = cache->IndexB[i]; - Vec2 wALocal = shapeA.GetVertex(v->indexA); - Vec2 wBLocal = shapeB.GetVertex(v->indexB); - v->wA = Common.Math.Mul(transformA, wALocal); - v->wB = Common.Math.Mul(transformB, wBLocal); - v->w = v->wB - v->wA; - v->a = 0.0f; - } - // Compute the new simplex metric, if it is substantially different than - // old metric then flush the simplex. - if (_count > 1) - { - float metric1 = cache->Metric; - float metric2 = GetMetric(); - if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Common.Settings.FLT_EPSILON) - { - // Reset the simplex. - _count = 0; - } - } - - // If the cache is empty or invalid ... - if (_count == 0) - { - SimplexVertex* v = vertices[0]; - v->indexA = 0; - v->indexB = 0; - Vec2 wALocal = shapeA.GetVertex(0); - Vec2 wBLocal = shapeB.GetVertex(0); - v->wA = Common.Math.Mul(transformA, wALocal); - v->wB = Common.Math.Mul(transformB, wBLocal); - v->w = v->wB - v->wA; - _count = 1; - } - } + // Else in edge region + lambda /= length; + x1 = p1s[1] + lambda * (p1s[0] - p1s[1]); + x2 = p2s[1] + lambda * (p2s[0] - p2s[1]); + return 2; } - internal unsafe void WriteCache(SimplexCache* cache) + // Possible regions: + // - points[2] + // - edge points[0]-points[2] + // - edge points[1]-points[2] + // - inside the triangle + public static int ProcessThree(out Vec2 x1, out Vec2 x2, ref Vec2[] p1s, ref Vec2[] p2s, + ref Vec2[] points) { - cache->Metric = GetMetric(); - cache->Count = (UInt16)_count; - SimplexVertex** vertices = stackalloc SimplexVertex*[3]; - fixed (SimplexVertex* v1Ptr = &_v1, v2Ptr = &_v2, v3Ptr = &_v3) - { - vertices[0] = v1Ptr; - vertices[1] = v2Ptr; - vertices[2] = v3Ptr; - for (int i = 0; i < _count; ++i) - { - cache->IndexA[i] = (Byte)(vertices[i]->indexA); - cache->IndexB[i] = (Byte)(vertices[i]->indexB); - } - } - } + Vec2 a = points[0]; + Vec2 b = points[1]; + Vec2 c = points[2]; - internal Vec2 GetClosestPoint() - { - switch (_count) - { - case 0: -#if DEBUG - Box2DXDebug.Assert(false); -#endif - return Vec2.Zero; - case 1: - return _v1.w; - case 2: - return _v1.a * _v1.w + _v2.a * _v2.w; - case 3: - return Vec2.Zero; - default: -#if DEBUG - Box2DXDebug.Assert(false); -#endif - return Vec2.Zero; - } - } + Vec2 ab = b - a; + Vec2 ac = c - a; + Vec2 bc = c - b; - internal unsafe void GetWitnessPoints(Vec2* pA, Vec2* pB) - { - switch (_count) - { - case 0: - Box2DXDebug.Assert(false); - break; - - case 1: - *pA = _v1.wA; - *pB = _v1.wB; - break; - - case 2: - *pA = _v1.a * _v1.wA + _v2.a * _v2.wA; - *pB = _v1.a * _v1.wB + _v2.a * _v2.wB; - break; - - case 3: - *pA = _v1.a * _v1.wA + _v2.a * _v2.wA + _v3.a * _v3.wA; - *pB = *pA; - break; - - default: - Box2DXDebug.Assert(false); - break; - } - } + float sn = -Vec2.Dot(a, ab), sd = Vec2.Dot(b, ab); + float tn = -Vec2.Dot(a, ac), td = Vec2.Dot(c, ac); + float un = -Vec2.Dot(b, bc), ud = Vec2.Dot(c, bc); - internal float GetMetric() - { - switch (_count) + // In vertex c region? + if (td <= 0.0f && ud <= 0.0f) { - case 0: -#if DEBUG - Box2DXDebug.Assert(false); -#endif - return 0.0f; + // Single point + x1 = p1s[2]; + x2 = p2s[2]; + p1s[0] = p1s[2]; + p2s[0] = p2s[2]; + points[0] = points[2]; + return 1; + } - case 1: - return 0.0f; + // Should not be in vertex a or b region. - case 2: - return Vec2.Distance(_v1.w, _v2.w); + //B2_NOT_USED(sd); + //B2_NOT_USED(sn); + Box2DXDebug.Assert(sn > 0.0f || tn > 0.0f); + Box2DXDebug.Assert(sd > 0.0f || un > 0.0f); - case 3: - return Vec2.Cross(_v2.w - _v1.w, _v3.w - _v1.w); + float n = Vec2.Cross(ab, ac); - default: -#if DEBUG - Box2DXDebug.Assert(false); +#if TARGET_FLOAT32_IS_FIXED + n = (n < 0.0f) ? -1.0f : ((n > 0.0f) ? 1.0f : 0.0f); #endif - return 0.0f; - } - } - // Solve a line segment using barycentric coordinates. - // - // p = a1 * w1 + a2 * w2 - // a1 + a2 = 1 - // - // The vector from the origin to the closest point on the line is - // perpendicular to the line. - // e12 = w2 - w1 - // dot(p, e) = 0 - // a1 * dot(w1, e) + a2 * dot(w2, e) = 0 - // - // 2-by-2 linear system - // [1 1 ][a1] = [1] - // [w1.e12 w2.e12][a2] = [0] - // - // Define - // d12_1 = dot(w2, e12) - // d12_2 = -dot(w1, e12) - // d12 = d12_1 + d12_2 - // - // Solution - // a1 = d12_1 / d12 - // a2 = d12_2 / d12 - internal void Solve2() - { - Vec2 w1 = _v1.w; - Vec2 w2 = _v2.w; - Vec2 e12 = w2 - w1; + // Should not be in edge ab region. + float vc = n * Vec2.Cross(a, b); + Box2DXDebug.Assert(vc > 0.0f || sn > 0.0f || sd > 0.0f); - // w1 region - float d12_2 = -Vec2.Dot(w1, e12); - if (d12_2 <= 0.0f) + // In edge bc region? + float va = n * Vec2.Cross(b, c); + if (va <= 0.0f && un >= 0.0f && ud >= 0.0f && (un + ud) > 0.0f) { - // a2 <= 0, so we clamp it to 0 - _v1.a = 1.0f; - _count = 1; - return; + Box2DXDebug.Assert(un + ud > 0.0f); + float lambda = un / (un + ud); + x1 = p1s[1] + lambda * (p1s[2] - p1s[1]); + x2 = p2s[1] + lambda * (p2s[2] - p2s[1]); + p1s[0] = p1s[2]; + p2s[0] = p2s[2]; + points[0] = points[2]; + return 2; } - // w2 region - float d12_1 = Vec2.Dot(w2, e12); - if (d12_1 <= 0.0f) + // In edge ac region? + float vb = n * Vec2.Cross(c, a); + if (vb <= 0.0f && tn >= 0.0f && td >= 0.0f && (tn + td) > 0.0f) { - // a1 <= 0, so we clamp it to 0 - _v2.a = 1.0f; - _count = 1; - _v1 = _v2; - return; + Box2DXDebug.Assert(tn + td > 0.0f); + float lambda = tn / (tn + td); + x1 = p1s[0] + lambda * (p1s[2] - p1s[0]); + x2 = p2s[0] + lambda * (p2s[2] - p2s[0]); + p1s[1] = p1s[2]; + p2s[1] = p2s[2]; + points[1] = points[2]; + return 2; } - // Must be in e12 region. - float inv_d12 = 1.0f / (d12_1 + d12_2); - _v1.a = d12_1 * inv_d12; - _v2.a = d12_2 * inv_d12; - _count = 2; + // Inside the triangle, compute barycentric coordinates + float denom = va + vb + vc; + Box2DXDebug.Assert(denom > 0.0f); + denom = 1.0f / denom; +#if TARGET_FLOAT32_IS_FIXED + x1 = denom * (va * p1s[0] + vb * p1s[1] + vc * p1s[2]); + x2 = denom * (va * p2s[0] + vb * p2s[1] + vc * p2s[2]); +#else + float u = va * denom; + float v = vb * denom; + float w = 1.0f - u - v; + x1 = u * p1s[0] + v * p1s[1] + w * p1s[2]; + x2 = u * p2s[0] + v * p2s[1] + w * p2s[2]; +#endif + return 3; } - // Possible regions: - // - points[2] - // - edge points[0]-points[2] - // - edge points[1]-points[2] - // - inside the triangle - internal void Solve3() + public static bool InPoints(Vec2 w, Vec2[] points, int pointCount) { - Vec2 w1 = _v1.w; - Vec2 w2 = _v2.w; - Vec2 w3 = _v3.w; - - // Edge12 - // [1 1 ][a1] = [1] - // [w1.e12 w2.e12][a2] = [0] - // a3 = 0 - Vec2 e12 = w2 - w1; - float w1e12 = Vec2.Dot(w1, e12); - float w2e12 = Vec2.Dot(w2, e12); - float d12_1 = w2e12; - float d12_2 = -w1e12; - - // Edge13 - // [1 1 ][a1] = [1] - // [w1.e13 w3.e13][a3] = [0] - // a2 = 0 - Vec2 e13 = w3 - w1; - float w1e13 = Vec2.Dot(w1, e13); - float w3e13 = Vec2.Dot(w3, e13); - float d13_1 = w3e13; - float d13_2 = -w1e13; - - // Edge23 - // [1 1 ][a2] = [1] - // [w2.e23 w3.e23][a3] = [0] - // a1 = 0 - Vec2 e23 = w3 - w2; - float w2e23 = Vec2.Dot(w2, e23); - float w3e23 = Vec2.Dot(w3, e23); - float d23_1 = w3e23; - float d23_2 = -w2e23; - - // Triangle123 - float n123 = Vec2.Cross(e12, e13); - - float d123_1 = n123 * Vec2.Cross(w2, w3); - float d123_2 = n123 * Vec2.Cross(w3, w1); - float d123_3 = n123 * Vec2.Cross(w1, w2); - - // w1 region - if (d12_2 <= 0.0f && d13_2 <= 0.0f) - { - _v1.a = 1.0f; - _count = 1; - return; - } - - // e12 - if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) + float k_tolerance = 100.0f * Common.Settings.FLT_EPSILON; + for (int i = 0; i < pointCount; ++i) { - float inv_d12 = 1.0f / (d12_1 + d12_2); - _v1.a = d12_1 * inv_d12; - _v2.a = d12_1 * inv_d12; - _count = 2; - return; - } - - // e13 - if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) - { - float inv_d13 = 1.0f / (d13_1 + d13_2); - _v1.a = d13_1 * inv_d13; - _v3.a = d13_2 * inv_d13; - _count = 2; - _v2 = _v3; - return; - } - - // w2 region - if (d12_1 <= 0.0f && d23_2 <= 0.0f) - { - _v2.a = 1.0f; - _count = 1; - _v1 = _v2; - return; - } + Vec2 d = Common.Math.Abs(w - points[i]); + Vec2 m = Common.Math.Max(Common.Math.Abs(w), Common.Math.Abs(points[i])); - // w3 region - if (d13_1 <= 0.0f && d23_1 <= 0.0f) - { - _v3.a = 1.0f; - _count = 1; - _v1 = _v3; - return; + if (d.X < k_tolerance * (m.X + 1.0f) && + d.Y < k_tolerance * (m.Y + 1.0f)) + { + return true; + } } - // e23 - if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) - { - float inv_d23 = 1.0f / (d23_1 + d23_2); - _v2.a = d23_1 * inv_d23; - _v3.a = d23_2 * inv_d23; - _count = 2; - _v1 = _v3; - return; - } + return false; + } - // Must be in triangle123 - float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); - _v1.a = d123_1 * inv_d123; - _v2.a = d123_2 * inv_d123; - _v3.a = d123_3 * inv_d123; - _count = 3; + public interface IGenericShape + { + Vec2 Support(XForm xf, Vec2 v); + Vec2 GetFirstVertex(XForm xf); } - } - public partial class Collision - { - /// - /// Compute the closest points between two shapes. Supports any combination of: - /// CircleShape, PolygonShape, EdgeShape. The simplex cache is input/output. - /// On the first call set SimplexCache.Count to zero. - /// - public unsafe static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB) + public static float DistanceGeneric(out Vec2 x1, out Vec2 x2, + T1 shape1_, XForm xf1, T2 shape2_, XForm xf2) { - output = new DistanceOutput(); + IGenericShape shape1 = shape1_ as IGenericShape; + IGenericShape shape2 = shape2_ as IGenericShape; - XForm transformA = input.TransformA; - XForm transformB = input.TransformB; + if (shape1 == null || shape2 == null) + Box2DXDebug.Assert(false, "Can not cast some parameters to IGenericShape"); - // Initialize the simplex. - Simplex simplex = new Simplex(); - fixed (SimplexCache* sPtr = &cache) - { - simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB); - } + Vec2[] p1s = new Vec2[3], p2s = new Vec2[3]; + Vec2[] points = new Vec2[3]; + int pointCount = 0; - // Get simplex vertices as an array. - SimplexVertex* vertices = &simplex._v1; + x1 = shape1.GetFirstVertex(xf1); + x2 = shape2.GetFirstVertex(xf2); - // These store the vertices of the last simplex so that we - // can check for duplicates and prevent cycling. - int* lastA = stackalloc int[4], lastB = stackalloc int[4]; - int lastCount; + float vSqr = 0.0f; + int maxIterations = 20; - // Main iteration loop. - int iter = 0; - const int k_maxIterationCount = 20; - while (iter < k_maxIterationCount) + for (int iter = 0; iter < maxIterations; ++iter) { - // Copy simplex so we can identify duplicates. - lastCount = simplex._count; - int i; - for (i = 0; i < lastCount; ++i) + Vec2 v = x2 - x1; + Vec2 w1 = shape1.Support(xf1, v); + Vec2 w2 = shape2.Support(xf2, -v); + + vSqr = Vec2.Dot(v, v); + Vec2 w = w2 - w1; + float vw = Vec2.Dot(v, w); + if (vSqr - vw <= 0.01f * vSqr || Collision.InPoints(w, points, pointCount)) // or w in points { - lastA[i] = vertices[i].indexA; - lastB[i] = vertices[i].indexB; + if (pointCount == 0) + { + x1 = w1; + x2 = w2; + } + Collision.GJKIterations = iter; + return Common.Math.Sqrt(vSqr); } - switch (simplex._count) + switch (pointCount) { - case 1: - break; - - case 2: - simplex.Solve2(); + case 0: + p1s[0] = w1; + p2s[0] = w2; + points[0] = w; + x1 = p1s[0]; + x2 = p2s[0]; + ++pointCount; break; - case 3: - simplex.Solve3(); + case 1: + p1s[1] = w1; + p2s[1] = w2; + points[1] = w; + pointCount = Collision.ProcessTwo(out x1, out x2, ref p1s, ref p2s, ref points); break; - default: -#if DEBUG - Box2DXDebug.Assert(false); -#endif + case 2: + p1s[2] = w1; + p2s[2] = w2; + points[2] = w; + pointCount = Collision.ProcessThree(out x1, out x2, ref p1s, ref p2s, ref points); break; } - // If we have 3 points, then the origin is in the corresponding triangle. - if (simplex._count == 3) + // If we have three points, then the origin is in the corresponding triangle. + if (pointCount == 3) { - break; + Collision.GJKIterations = iter; + return 0.0f; } - // Compute closest point. - Vec2 p = simplex.GetClosestPoint(); - float distanceSqr = p.LengthSquared(); - - // Ensure the search direction is numerically fit. - if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED) + float maxSqr = -Common.Settings.FLT_MAX; + for (int i = 0; i < pointCount; ++i) { - // The origin is probably contained by a line segment - // or triangle. Thus the shapes are overlapped. - - // We can't return zero here even though there may be overlap. - // In case the simplex is a point, segment, or triangle it is difficult - // to determine if the origin is contained in the CSO or very close to it. - break; + maxSqr = Common.Math.Max(maxSqr, Vec2.Dot(points[i], points[i])); } - // Compute a tentative new simplex vertex using support points. - SimplexVertex* vertex = vertices + simplex._count; - vertex->indexA = shapeA.GetSupport(Common.Math.MulT(transformA.R, p)); - vertex->wA = Common.Math.Mul(transformA, shapeA.GetVertex(vertex->indexA)); - //Vec2 wBLocal; - vertex->indexB = shapeB.GetSupport(Common.Math.MulT(transformB.R, -p)); - vertex->wB = Common.Math.Mul(transformB, shapeB.GetVertex(vertex->indexB)); - vertex->w = vertex->wB - vertex->wA; - - // Iteration count is equated to the number of support point calls. - ++iter; - - // Check for convergence. - float lowerBound = Vec2.Dot(p, vertex->w); - float upperBound = distanceSqr; - const float k_relativeTolSqr = 0.01f * 0.01f; // 1:100 - if (upperBound - lowerBound <= k_relativeTolSqr * upperBound) +#if TARGET_FLOAT32_IS_FIXED + if (pointCount == 3 || vSqr <= 5.0*Common.Settings.FLT_EPSILON * maxSqr) +#else + if (vSqr <= 100.0f * Common.Settings.FLT_EPSILON * maxSqr) +#endif { - // Converged! - break; - } + Collision.GJKIterations = iter; + v = x2 - x1; + vSqr = Vec2.Dot(v, v); - // Check for duplicate support points. - bool duplicate = false; - for (i = 0; i < lastCount; ++i) - { - if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i]) - { - duplicate = true; - break; - } + return Common.Math.Sqrt(vSqr); } + } - // If we found a duplicate support point we must exit to avoid cycling. - if (duplicate) - { - break; - } + Collision.GJKIterations = maxIterations; + return Common.Math.Sqrt(vSqr); + } - // New vertex is ok and needed. - ++simplex._count; + public static float DistanceCC(out Vec2 x1, out Vec2 x2, + CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2) + { + Vec2 p1 = Common.Math.Mul(xf1, circle1.GetLocalPosition()); + Vec2 p2 = Common.Math.Mul(xf2, circle2.GetLocalPosition()); + + Vec2 d = p2 - p1; + float dSqr = Vec2.Dot(d, d); + float r1 = circle1.GetRadius() - Settings.ToiSlop; + float r2 = circle2.GetRadius() - Settings.ToiSlop; + float r = r1 + r2; + if (dSqr > r * r) + { + float dLen = d.Normalize(); + float distance = dLen - r; + x1 = p1 + r1 * d; + x2 = p2 - r2 * d; + return distance; + } + else if (dSqr > Common.Settings.FLT_EPSILON * Common.Settings.FLT_EPSILON) + { + d.Normalize(); + x1 = p1 + r1 * d; + x2 = x1; + return 0.0f; } + x1 = p1; + x2 = x1; + return 0.0f; + } - fixed (DistanceOutput* doPtr = &output) +#warning "CAS" + // This is used for polygon-vs-circle distance. + public class Point : Collision.IGenericShape + { + public Vec2 p; + + public Vec2 Support(XForm xf, Vec2 v) { - // Prepare output. - simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB); - doPtr->Distance = Vec2.Distance(doPtr->PointA, doPtr->PointB); - doPtr->Iterations = iter; + return p; } - fixed (SimplexCache* sPtr = &cache) + public Vec2 GetFirstVertex(XForm xf) { - // Cache the simplex. - simplex.WriteCache(sPtr); + return p; } + } - // Apply radii if requested. - if (input.UseRadii) + // GJK is more robust with polygon-vs-point than polygon-vs-circle. + // So we convert polygon-vs-circle to polygon-vs-point. + public static float DistancePC(out Vec2 x1, out Vec2 x2, + PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2) + { + Point point = new Point(); + point.p = Common.Math.Mul(xf2, circle.GetLocalPosition()); + + float distance = DistanceGeneric(out x1, out x2, polygon, xf1, point, XForm.Identity); + + float r = circle.GetRadius() - Settings.ToiSlop; + + if (distance > r) { - float rA = shapeA._radius; - float rB = shapeB._radius; + distance -= r; + Vec2 d = x2 - x1; + d.Normalize(); + x2 -= r * d; + } + else + { + distance = 0.0f; + x2 = x1; + } - if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_EPSILON) - { - // Shapes are still no overlapped. - // Move the witness points to the outer surface. - output.Distance -= rA + rB; - Vec2 normal = output.PointB - output.PointA; - normal.Normalize(); - output.PointA += rA * normal; - output.PointB -= rB * normal; - } - else - { - // Shapes are overlapped when radii are considered. - // Move the witness points to the middle. - Vec2 p = 0.5f * (output.PointA + output.PointB); - output.PointA = p; - output.PointB = p; - output.Distance = 0.0f; - } + return distance; + } + + public static float Distance(out Vec2 x1, out Vec2 x2, + Shape shape1, XForm xf1, Shape shape2, XForm xf2) + { + x1 = new Vec2(); + x2 = new Vec2(); + + ShapeType type1 = shape1.GetType(); + ShapeType type2 = shape2.GetType(); + + if (type1 == ShapeType.CircleShape && type2 == ShapeType.CircleShape) + { + return DistanceCC(out x1, out x2, (CircleShape)shape1, xf1, (CircleShape)shape2, xf2); + } + + if (type1 == ShapeType.PolygonShape && type2 == ShapeType.CircleShape) + { + return DistancePC(out x1, out x2, (PolygonShape)shape1, xf1, (CircleShape)shape2, xf2); + } + + if (type1 == ShapeType.CircleShape && type2 == ShapeType.PolygonShape) + { + return DistancePC(out x2, out x1, (PolygonShape)shape2, xf2, (CircleShape)shape1, xf1); } + + if (type1 == ShapeType.PolygonShape && type2 == ShapeType.PolygonShape) + { + return DistanceGeneric(out x1, out x2, (PolygonShape)shape1, xf1, (PolygonShape)shape2, xf2); + } + + return 0.0f; } } } \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.TimeOfImpact.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.TimeOfImpact.cs index b59f19b..a28a74c 100644 --- a/Box2d/Assets/Program/Box2d/Collision/Collision.TimeOfImpact.cs +++ b/Box2d/Assets/Program/Box2d/Collision/Collision.TimeOfImpact.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -19,339 +19,106 @@ 3. This notice may not be removed or altered from any source distribution. */ +using System; +using System.Collections.Generic; +using System.Text; + using Box2DX.Common; namespace Box2DX.Collision { - /// - /// Inpute parameters for TimeOfImpact - /// - public struct TOIInput - { - public Sweep SweepA; - public Sweep SweepB; - public float SweepRadiusA; - public float SweepRadiusB; - public float Tolerance; - } - - internal struct SeparationFunction - { - internal enum Type - { - Points, - FaceA, - FaceB - }; - - internal unsafe void Initialize(SimplexCache* cache, - Shape shapeA, XForm transformA, - Shape shapeB, XForm transformB) - { - ShapeA = shapeA; - ShapeB = shapeB; - int count = cache->Count; - Box2DXDebug.Assert(0 < count && count < 3); - - if (count == 1) - { - FaceType = Type.Points; - Vec2 localPointA = ShapeA.GetVertex(cache->IndexA[0]); - Vec2 localPointB = ShapeB.GetVertex(cache->IndexB[0]); - Vec2 pointA = Common.Math.Mul(transformA, localPointA); - Vec2 pointB = Common.Math.Mul(transformB, localPointB); - Axis = pointB - pointA; - Axis.Normalize(); - } - else if (cache->IndexB[0] == cache->IndexB[1]) - { - // Two points on A and one on B - FaceType = Type.FaceA; - Vec2 localPointA1 = ShapeA.GetVertex(cache->IndexA[0]); - Vec2 localPointA2 = ShapeA.GetVertex(cache->IndexA[1]); - Vec2 localPointB = ShapeB.GetVertex(cache->IndexB[0]); - LocalPoint = 0.5f * (localPointA1 + localPointA2); - Axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f); - Axis.Normalize(); - - Vec2 normal = Common.Math.Mul(transformA.R, Axis); - Vec2 pointA = Common.Math.Mul(transformA, LocalPoint); - Vec2 pointB = Common.Math.Mul(transformB, localPointB); - - float s = Vec2.Dot(pointB - pointA, normal); - if (s < 0.0f) - { - Axis = -Axis; - } - } - else - { - // Two points on B and one or two points on A. - // We ignore the second point on A. - FaceType = Type.FaceB; - Vec2 localPointA = shapeA.GetVertex(cache->IndexA[0]); - Vec2 localPointB1 = shapeB.GetVertex(cache->IndexB[0]); - Vec2 localPointB2 = shapeB.GetVertex(cache->IndexB[1]); - LocalPoint = 0.5f * (localPointB1 + localPointB2); - Axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f); - Axis.Normalize(); - - Vec2 normal = Common.Math.Mul(transformB.R, Axis); - Vec2 pointB = Common.Math.Mul(transformB, LocalPoint); - Vec2 pointA = Common.Math.Mul(transformA, localPointA); - - float s = Vec2.Dot(pointA - pointB, normal); - if (s < 0.0f) - { - Axis = -Axis; - } - } - } - - internal float Evaluate(XForm transformA, XForm transformB) - { - switch (FaceType) - { - case Type.Points: - { - Vec2 axisA = Common.Math.MulT(transformA.R, Axis); - Vec2 axisB = Common.Math.MulT(transformB.R, -Axis); - Vec2 localPointA = ShapeA.GetSupportVertex(axisA); - Vec2 localPointB = ShapeB.GetSupportVertex(axisB); - Vec2 pointA = Common.Math.Mul(transformA, localPointA); - Vec2 pointB = Common.Math.Mul(transformB, localPointB); - float separation = Vec2.Dot(pointB - pointA, Axis); - return separation; - } - - case Type.FaceA: - { - Vec2 normal = Common.Math.Mul(transformA.R, Axis); - Vec2 pointA = Common.Math.Mul(transformA, LocalPoint); - - Vec2 axisB = Common.Math.MulT(transformB.R, -normal); - - Vec2 localPointB = ShapeB.GetSupportVertex(axisB); - Vec2 pointB = Common.Math.Mul(transformB, localPointB); - - float separation = Vec2.Dot(pointB - pointA, normal); - return separation; - } - - case Type.FaceB: - { - Vec2 normal = Common.Math.Mul(transformB.R, Axis); - Vec2 pointB = Common.Math.Mul(transformB, LocalPoint); - - Vec2 axisA = Common.Math.MulT(transformA.R, -normal); - - Vec2 localPointA = ShapeA.GetSupportVertex(axisA); - Vec2 pointA = Common.Math.Mul(transformA, localPointA); - - float separation = Vec2.Dot(pointA - pointB, normal); - return separation; - } - - default: - Box2DXDebug.Assert(false); - return 0.0f; - } - } - - internal Shape ShapeA; - internal Shape ShapeB; - internal Type FaceType; - internal Vec2 LocalPoint; - internal Vec2 Axis; - } - public partial class Collision - { - public static int MaxToiIters; - public static int MaxToiRootIters; - - // CCD via the secant method. + { + // This algorithm uses conservative advancement to compute the time of + // impact (TOI) of two shapes. + // Refs: Bullet, Young Kim /// /// Compute the time when two shapes begin to touch or touch at a closer distance. - /// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance. - /// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be - /// smaller than sum of the shape radii. - /// Warning the sweeps must have the same time interval. + /// warning the sweeps must have the same time interval. /// + /// + /// + /// + /// /// /// The fraction between [0,1] in which the shapes first touch. /// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch. /// - public static float TimeOfImpact(TOIInput input, Shape shapeA, Shape shapeB) +#warning: "check params" + public static float TimeOfImpact(Shape shape1, Sweep sweep1, Shape shape2, Sweep sweep2) { - Sweep sweepA = input.SweepA; - Sweep sweepB = input.SweepB; + float r1 = shape1.GetSweepRadius(); + float r2 = shape2.GetSweepRadius(); - Box2DXDebug.Assert(sweepA.T0 == sweepB.T0); - Box2DXDebug.Assert(1.0f - sweepA.T0 > Common.Settings.FLT_EPSILON); + Box2DXDebug.Assert(sweep1.T0 == sweep2.T0); + Box2DXDebug.Assert(1.0f - sweep1.T0 > Common.Settings.FLT_EPSILON); - float radius = shapeA._radius + shapeB._radius; - float tolerance = input.Tolerance; + float t0 = sweep1.T0; + Vec2 v1 = sweep1.C - sweep1.C0; + Vec2 v2 = sweep2.C - sweep2.C0; + float omega1 = sweep1.A - sweep1.A0; + float omega2 = sweep2.A - sweep2.A0; float alpha = 0.0f; - const int k_maxIterations = 1000; // TODO_ERIN b2Settings + Vec2 p1, p2; + int k_maxIterations = 20; // TODO_ERIN b2Settings int iter = 0; - float target = 0.0f; - - // Prepare input for distance query. - SimplexCache cache = new SimplexCache(); - cache.Count = 0; - DistanceInput distanceInput; - distanceInput.UseRadii = false; + Vec2 normal = Vec2.Zero; + float distance = 0.0f; + float targetDistance = 0.0f; for (; ; ) { - XForm xfA, xfB; - sweepA.GetTransform(out xfA, alpha); - sweepB.GetTransform(out xfB, alpha); + float t = (1.0f - alpha) * t0 + alpha; + XForm xf1, xf2; + sweep1.GetXForm(out xf1, t); + sweep2.GetXForm(out xf2, t); // Get the distance between shapes. - distanceInput.TransformA = xfA; - distanceInput.TransformB = xfB; - DistanceOutput distanceOutput; - Distance(out distanceOutput, ref cache, ref distanceInput, shapeA, shapeB); - - if (distanceOutput.Distance <= 0.0f) - { - alpha = 1.0f; - break; - } - - SeparationFunction fcn = new SeparationFunction(); - unsafe - { - fcn.Initialize(&cache, shapeA, xfA, shapeB, xfB); - } - - float separation = fcn.Evaluate(xfA, xfB); - if (separation <= 0.0f) - { - alpha = 1.0f; - break; - } + distance = Collision.Distance(out p1, out p2, shape1, xf1, shape2, xf2); if (iter == 0) { // Compute a reasonable target distance to give some breathing room - // for conservative advancement. We take advantage of the shape radii - // to create additional clearance. - if (separation > radius) + // for conservative advancement. + if (distance > 2.0f * Settings.ToiSlop) { - target = Common.Math.Max(radius - tolerance, 0.75f * radius); + targetDistance = 1.5f * Settings.ToiSlop; } else { - target = Common.Math.Max(separation - tolerance, 0.02f * radius); + targetDistance = Common.Math.Max(0.05f * Settings.ToiSlop, distance - 0.5f * Settings.ToiSlop); } } - if (separation - target < 0.5f * tolerance) + if (distance - targetDistance < 0.05f * Settings.ToiSlop || iter == k_maxIterations) { - if (iter == 0) - { - alpha = 1.0f; - break; - } - break; } -#if _FALSE - // Dump the curve seen by the root finder - { - const int32 N = 100; - float32 dx = 1.0f / N; - float32 xs[N+1]; - float32 fs[N+1]; - - float32 x = 0.0f; - - for (int32 i = 0; i <= N; ++i) - { - sweepA.GetTransform(&xfA, x); - sweepB.GetTransform(&xfB, x); - float32 f = fcn.Evaluate(xfA, xfB) - target; - - printf("%g %g\n", x, f); - - xs[i] = x; - fs[i] = f; - - x += dx; - } - } -#endif + normal = p2 - p1; + normal.Normalize(); - // Compute 1D root of: f(x) - target = 0 - float newAlpha = alpha; + // Compute upper bound on remaining movement. + float approachVelocityBound = Vec2.Dot(normal, v1 - v2) + + Common.Math.Abs(omega1) * r1 + Common.Math.Abs(omega2) * r2; + if (Common.Math.Abs(approachVelocityBound) < Common.Settings.FLT_EPSILON) { - float x1 = alpha, x2 = 1.0f; - - float f1 = separation; - - sweepA.GetTransform(out xfA, x2); - sweepB.GetTransform(out xfB, x2); - float f2 = fcn.Evaluate(xfA, xfB); - - // If intervals don't overlap at t2, then we are done. - if (f2 >= target) - { - alpha = 1.0f; - break; - } - - // Determine when intervals intersect. - int rootIterCount = 0; - for (; ; ) - { - // Use a mix of the secant rule and bisection. - float x; - if ((rootIterCount & 1) != 0) - { - // Secant rule to improve convergence. - x = x1 + (target - f1) * (x2 - x1) / (f2 - f1); - } - else - { - // Bisection to guarantee progress. - x = 0.5f * (x1 + x2); - } - - sweepA.GetTransform(out xfA, x); - sweepB.GetTransform(out xfB, x); - - float f = fcn.Evaluate(xfA, xfB); - - if (Common.Math.Abs(f - target) < 0.025f * tolerance) - { - newAlpha = x; - break; - } - - // Ensure we continue to bracket the root. - if (f > target) - { - x1 = x; - f1 = f; - } - else - { - x2 = x; - f2 = f; - } - - ++rootIterCount; + alpha = 1.0f; + break; + } - Box2DXDebug.Assert(rootIterCount < 50); - } + // Get the conservative time increment. Don't advance all the way. + float dAlpha = (distance - targetDistance) / approachVelocityBound; + //float dt = (distance - 0.5f * Settings.LinearSlop) / approachVelocityBound; + float newAlpha = alpha + dAlpha; - MaxToiRootIters = Common.Math.Max(MaxToiRootIters, rootIterCount); + // The shapes may be moving apart or a safe distance apart. + if (newAlpha < 0.0f || 1.0f < newAlpha) + { + alpha = 1.0f; + break; } // Ensure significant advancement. @@ -363,15 +130,8 @@ namespace Box2DX.Collision alpha = newAlpha; ++iter; - - if (iter == k_maxIterations) - { - break; - } } - MaxToiIters = Common.Math.Max(MaxToiIters, iter); - return alpha; } } diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.cs index e77e23b..6a88ab2 100644 --- a/Box2d/Assets/Program/Box2d/Collision/Collision.cs +++ b/Box2d/Assets/Program/Box2d/Collision/Collision.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -20,6 +20,9 @@ */ using System; +using System.Collections.Generic; +using System.Text; + using Box2DX.Common; namespace Box2DX.Collision @@ -45,90 +48,6 @@ namespace Box2DX.Collision return true; } - - /// - /// Compute the point states given two manifolds. The states pertain to the transition from manifold1 - /// to manifold2. So state1 is either persist or remove while state2 is either add or persist. - /// - public static void GetPointStates(PointState[/*b2_maxManifoldPoints*/] state1, PointState[/*b2_maxManifoldPoints*/] state2, - Manifold manifold1, Manifold manifold2) - { - for (int i = 0; i < Common.Settings.MaxManifoldPoints; ++i) - { - state1[i] = PointState.NullState; - state2[i] = PointState.NullState; - } - - // Detect persists and removes. - for (int i = 0; i < manifold1.PointCount; ++i) - { - ContactID id = manifold1.Points[i].ID; - - state1[i] = PointState.RemoveState; - - for (int j = 0; j < manifold2.PointCount; ++j) - { - if (manifold2.Points[j].ID.Key == id.Key) - { - state1[i] = PointState.PersistState; - break; - } - } - } - - // Detect persists and adds. - for (int i = 0; i < manifold2.PointCount; ++i) - { - ContactID id = manifold2.Points[i].ID; - - state2[i] = PointState.AddState; - - for (int j = 0; j < manifold1.PointCount; ++j) - { - if (manifold1.Points[j].ID.Key == id.Key) - { - state2[i] = PointState.PersistState; - break; - } - } - } - } - - // Sutherland-Hodgman clipping. - public static int ClipSegmentToLine(out ClipVertex[/*2*/] vOut, ClipVertex[/*2*/] vIn, Vec2 normal, float offset) - { - vOut = new ClipVertex[2]; - - // Start with no output points - int numOut = 0; - - // Calculate the distance of end points to the line - float distance0 = Vec2.Dot(normal, vIn[0].V) - offset; - float distance1 = Vec2.Dot(normal, vIn[1].V) - offset; - - // If the points are behind the plane - if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; - if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; - - // If the points are on different sides of the plane - if (distance0 * distance1 < 0.0f) - { - // Find intersection point of edge and plane - float 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; - } - ++numOut; - } - - return numOut; - } } /// @@ -139,22 +58,22 @@ namespace Box2DX.Collision /// /// The edge that defines the outward contact normal. /// - public Byte ReferenceEdge; + public byte ReferenceEdge; /// /// The edge most anti-parallel to the reference edge. /// - public Byte IncidentEdge; + public byte IncidentEdge; /// /// The vertex (0 or 1) on the incident edge that was clipped. /// - public Byte IncidentVertex; + public byte IncidentVertex; /// /// A value of 1 indicates that the reference edge is on shape2. /// - public Byte Flip; + public byte Flip; } /// @@ -170,27 +89,33 @@ namespace Box2DX.Collision /// Used to quickly compare contact ids. /// [System.Runtime.InteropServices.FieldOffset(0)] - public UInt32 Key; + public uint Key; } +#warning "CAS" /// /// A manifold point is a contact point belonging to a contact /// manifold. It holds details related to the geometry and dynamics /// of the contact points. - /// The local point usage depends on the manifold type: - /// -Circles: the local center of circleB - /// -FaceA: the local center of cirlceB or the clip point of polygonB - /// -FaceB: the clip point of polygonA - /// This structure is stored across time steps, so we keep it small. - /// Note: the impulses are used for internal caching and may not - /// provide reliable contact forces, especially for high speed collisions. + /// The point is stored in local coordinates because CCD + /// requires sub-stepping in which the separation is stale. /// public class ManifoldPoint { /// - /// Usage depends on manifold type. + /// Local position of the contact point in body1. /// - public Vec2 LocalPoint; + public Vec2 LocalPoint1; + + /// + /// Local position of the contact point in body2. + /// + public Vec2 LocalPoint2; + + /// + /// The separation of the shapes along the normal vector. + /// + public float Separation; /// /// The non-penetration impulse. @@ -210,7 +135,9 @@ namespace Box2DX.Collision public ManifoldPoint Clone() { ManifoldPoint newPoint = new ManifoldPoint(); - newPoint.LocalPoint = this.LocalPoint; + newPoint.LocalPoint1 = this.LocalPoint1; + newPoint.LocalPoint2 = this.LocalPoint2; + newPoint.Separation = this.Separation; newPoint.NormalImpulse = this.NormalImpulse; newPoint.TangentImpulse = this.TangentImpulse; newPoint.ID = this.ID; @@ -218,13 +145,7 @@ namespace Box2DX.Collision } } - public enum ManifoldType - { - Circles, - FaceA, - FaceB - } - +#warning "CAS" /// /// A manifold for two touching convex shapes. /// @@ -235,14 +156,10 @@ namespace Box2DX.Collision /// public ManifoldPoint[/*Settings.MaxManifoldPoints*/] Points = new ManifoldPoint[Settings.MaxManifoldPoints]; - public Vec2 LocalPlaneNormal; - /// - /// Usage depends on manifold type. + /// The shared unit normal vector. /// - public Vec2 LocalPoint; - - public ManifoldType Type; + public Vec2 Normal; /// /// The number of manifold points. @@ -258,9 +175,7 @@ namespace Box2DX.Collision public Manifold Clone() { Manifold newManifold = new Manifold(); - newManifold.LocalPlaneNormal = this.LocalPlaneNormal; - newManifold.LocalPoint = this.LocalPoint; - newManifold.Type = this.Type; + newManifold.Normal = this.Normal; newManifold.PointCount = this.PointCount; int pointCount = this.Points.Length; ManifoldPoint[] tmp = new ManifoldPoint[pointCount]; @@ -296,6 +211,11 @@ namespace Box2DX.Collision /// /// Ray cast against this segment with another segment. /// + /// + /// + /// + /// + /// public bool TestSegment(out float lambda, out Vec2 normal, Segment segment, float maxLambda) { lambda = 0f; @@ -351,16 +271,6 @@ namespace Box2DX.Collision /// public struct AABB { - /// - /// The lower vertex. - /// - public Vec2 LowerBound; - - /// - /// The upper vertex. - /// - public Vec2 UpperBound; - /// Verify that the bounds are sorted. public bool IsValid { @@ -373,251 +283,36 @@ namespace Box2DX.Collision } } - /// Get the center of the AABB. - public Vec2 Center - { - get { return 0.5f * (LowerBound + UpperBound); } - } - - /// Get the extents of the AABB (half-widths). - public Vec2 Extents - { - get { return 0.5f * (UpperBound - LowerBound); } - } - - /// Combine two AABBs into this one. - public void Combine(AABB aabb1, AABB aabb2) - { - LowerBound = Common.Math.Min(aabb1.LowerBound, aabb2.LowerBound); - UpperBound = Common.Math.Max(aabb1.UpperBound, aabb2.UpperBound); - } - - /// Does this aabb contain the provided AABB. - public bool Contains(AABB aabb) - { - bool result = LowerBound.X <= aabb.LowerBound.X; - result = result && LowerBound.Y <= aabb.LowerBound.Y; - result = result && aabb.UpperBound.X <= UpperBound.X; - result = result && aabb.UpperBound.Y <= UpperBound.Y; - return result; - } /// - // From Real-time Collision Detection, p179. + /// The lower vertex. /// - public void RayCast(out RayCastOutput output, RayCastInput input) - { - float tmin = -Common.Settings.FLT_MAX; - float tmax = Common.Settings.FLT_MAX; - - output = new RayCastOutput(); - - output.Hit = false; - - Vec2 p = input.P1; - Vec2 d = input.P2 - input.P1; - Vec2 absD = Common.Math.Abs(d); - - Vec2 normal = new Vec2(0); - - for (int i = 0; i < 2; ++i) - { - if (absD[i] < Common.Settings.FLT_EPSILON) - { - // Parallel. - if (p[i] < LowerBound[i] || UpperBound[i] < p[i]) - { - return; - } - } - else - { - float inv_d = 1.0f / d[i]; - float t1 = (LowerBound[i] - p[i]) * inv_d; - float t2 = (UpperBound[i] - p[i]) * inv_d; - - // Sign of the normal vector. - float s = -1.0f; - - if (t1 > t2) - { - Common.Math.Swap(ref t1, ref t2); - s = 1.0f; - } - - // Push the min up - if (t1 > tmin) - { - normal.SetZero(); - normal[i] = s; - tmin = t1; - } - - // Pull the max down - tmax = Common.Math.Min(tmax, t2); - - if (tmin > tmax) - { - return; - } - } - } - - // Does the ray start inside the box? - // Does the ray intersect beyond the max fraction? - if (tmin < 0.0f || input.MaxFraction < tmin) - { - return; - } - - // Intersection. - output.Fraction = tmin; - output.Normal = normal; - output.Hit = true; - } - } + public Vec2 LowerBound; - /// - /// This is used for determining the state of contact points. - /// - public enum PointState - { /// - /// Point does not exist. - /// - NullState, - /// - /// Point was added in the update. - /// - AddState, - /// - /// Point persisted across the update. - /// - PersistState, - /// - ///Point was removed in the update. + /// The upper vertex. /// - RemoveState + public Vec2 UpperBound; } /// - /// This is used to compute the current state of a contact manifold. + /// An oriented bounding box. /// - public class WorldManifold + public struct OBB { /// - /// World vector pointing from A to B. + /// The rotation matrix. /// - public Vec2 Normal; + public Mat22 R; + /// - /// World contact point (point of intersection). + /// The local centroid. /// - public Vec2[] Points = new Vec2[Common.Settings.MaxManifoldPoints]; + public Vec2 Center; - public WorldManifold Clone() - { - WorldManifold newManifold = new WorldManifold(); - newManifold.Normal = this.Normal; - this.Points.CopyTo(newManifold.Points, 0); - return newManifold; - } - - /// Evaluate the manifold with supplied transforms. This assumes - /// modest motion from the original state. This does not change the - /// point count, impulses, etc. The radii must come from the shapes - /// that generated the manifold. - public void Initialize(Manifold manifold, XForm xfA, float radiusA, XForm xfB, float radiusB) - { - if (manifold.PointCount == 0) - { - return; - } - - switch (manifold.Type) - { - case ManifoldType.Circles: - { - Vec2 pointA = Common.Math.Mul(xfA, manifold.LocalPoint); - Vec2 pointB = Common.Math.Mul(xfB, manifold.Points[0].LocalPoint); - Vec2 normal = new Vec2(1.0f, 0.0f); - if (Vec2.DistanceSquared(pointA, pointB) > Common.Settings.FLT_EPSILON_SQUARED) - { - normal = pointB - pointA; - normal.Normalize(); - } - - Normal = normal; - - Vec2 cA = pointA + radiusA * normal; - Vec2 cB = pointB - radiusB * normal; - Points[0] = 0.5f * (cA + cB); - } - break; - - case ManifoldType.FaceA: - { - Vec2 normal = Common.Math.Mul(xfA.R, manifold.LocalPlaneNormal); - Vec2 planePoint = Common.Math.Mul(xfA, manifold.LocalPoint); - - // Ensure normal points from A to B. - Normal = normal; - - for (int i = 0; i < manifold.PointCount; ++i) - { - Vec2 clipPoint = Common.Math.Mul(xfB, manifold.Points[i].LocalPoint); - Vec2 cA = clipPoint + (radiusA - Vec2.Dot(clipPoint - planePoint, normal)) * normal; - Vec2 cB = clipPoint - radiusB * normal; - Points[i] = 0.5f * (cA + cB); - } - } - break; - - case ManifoldType.FaceB: - { - Vec2 normal = Common.Math.Mul(xfB.R, manifold.LocalPlaneNormal); - Vec2 planePoint = Common.Math.Mul(xfB, manifold.LocalPoint); - - // Ensure normal points from A to B. - Normal = -normal; - - for (int i = 0; i < manifold.PointCount; ++i) - { - Vec2 clipPoint = Common.Math.Mul(xfA, manifold.Points[i].LocalPoint); - Vec2 cA = clipPoint - radiusA * normal; - Vec2 cB = clipPoint + (radiusB - Vec2.Dot(clipPoint - planePoint, normal)) * normal; - Points[i] = 0.5f * (cA + cB); - } - } - break; - } - } - } - - /// - /// Used for computing contact manifolds. - /// - public struct ClipVertex - { - public Vec2 V; - public ContactID ID; - } - - /// - /// Ray-cast input data. - /// - public struct RayCastInput - { - public Vec2 P1, P2; - public float MaxFraction; - } - - /// - /// Ray-cast output data. - /// - public struct RayCastOutput - { - public Vec2 Normal; - public float Fraction; - public bool Hit; + /// + /// The half-widths. + /// + public Vec2 Extents; } } \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/PairManager.cs b/Box2d/Assets/Program/Box2d/Collision/PairManager.cs index 0fcb8ab..361fee5 100644 --- a/Box2d/Assets/Program/Box2d/Collision/PairManager.cs +++ b/Box2d/Assets/Program/Box2d/Collision/PairManager.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -26,10 +26,15 @@ #define DEBUG using System; +using System.Collections; +using System.Collections.Generic; +using System.Text; + using Box2DX.Common; namespace Box2DX.Collision { +#warning "CAS" public class Pair { [Flags] @@ -104,12 +109,12 @@ namespace Box2DX.Collision _freePair = 0; for (int i = 0; i < Settings.MaxPairs; ++i) { - _pairs[i] = new Pair();//todo: need some pool here + _pairs[i] = new Pair(); _pairs[i].ProxyId1 = PairManager.NullProxy; _pairs[i].ProxyId2 = PairManager.NullProxy; _pairs[i].UserData = null; _pairs[i].Status = 0; - _pairs[i].Next = (ushort)(i + 1U); + _pairs[i].Next = (ushort)(i + (ushort)1); } _pairs[Settings.MaxPairs - 1].Next = PairManager.NullPair; _pairCount = 0; @@ -475,15 +480,15 @@ namespace Box2DX.Collision public static int BufferedPairSortPredicate(BufferedPair pair1, BufferedPair pair2) { - if (pair1.ProxyId1 < pair2.ProxyId1) + if (pair1.ProxyId1 > pair2.ProxyId1) return 1; - else if (pair1.ProxyId1 > pair2.ProxyId1) + else if (pair1.ProxyId1 < pair2.ProxyId1) return -1; else { - if (pair1.ProxyId2 < pair2.ProxyId2) + if (pair1.ProxyId2 > pair2.ProxyId2) return 1; - else if (pair1.ProxyId2 > pair2.ProxyId2) + else if (pair1.ProxyId2 < pair2.ProxyId2) return -1; } diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs b/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs index 553accd..5e1fa30 100644 --- a/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -19,26 +19,62 @@ 3. This notice may not be removed or altered from any source distribution. */ +using System; +using System.Collections.Generic; +using System.Text; + using Box2DX.Common; namespace Box2DX.Collision { + /// + /// This structure is used to build circle shapes. + /// + public class CircleDef : ShapeDef + { + public Vec2 LocalPosition; + public float Radius; + + public CircleDef() + { + Type = ShapeType.CircleShape; + LocalPosition = Vec2.Zero; + Radius = 1.0f; + } + } + /// /// A circle shape. /// public class CircleShape : Shape { - // Position - internal Vec2 _position; + // Local position in parent body + private Vec2 _localPosition; + // Radius of this circle. + private float _radius; - public CircleShape() + internal CircleShape(ShapeDef def) + : base(def) { + Box2DXDebug.Assert(def.Type == ShapeType.CircleShape); + CircleDef circleDef = (CircleDef)def; + _type = ShapeType.CircleShape; + _localPosition = circleDef.LocalPosition; + _radius = circleDef.Radius; + } + + internal override void UpdateSweepRadius(Vec2 center) + { + // Update the sweep radius (maximum radius) as measured from + // a local center point. + Vec2 d = _localPosition - center; + _sweepRadius = d.Length() + _radius - Settings.ToiSlop; } public override bool TestPoint(XForm transform, Vec2 p) { - Vec2 center = transform.Position + Common.Math.Mul(transform.R, _position); + Vec2 center = transform.Position + Common.Math.Mul(transform.R, _localPosition); Vec2 d = p - center; return Vec2.Dot(d, d) <= _radius * _radius; } @@ -52,7 +88,7 @@ namespace Box2DX.Collision lambda = 0f; normal = Vec2.Zero; - Vec2 position = transform.Position + Common.Math.Mul(transform.R, _position); + Vec2 position = transform.Position + Common.Math.Mul(transform.R, _localPosition); Vec2 s = segment.P1 - position; float b = Vec2.Dot(s, s) - _radius * _radius; @@ -95,85 +131,51 @@ namespace Box2DX.Collision { aabb = new AABB(); - Vec2 p = transform.Position + Common.Math.Mul(transform.R, _position); + Vec2 p = transform.Position + Common.Math.Mul(transform.R, _localPosition); aabb.LowerBound.Set(p.X - _radius, p.Y - _radius); aabb.UpperBound.Set(p.X + _radius, p.Y + _radius); } - public override void ComputeMass(out MassData massData, float density) + public override void ComputeSweptAABB(out AABB aabb, XForm transform1, XForm transform2) { - massData = new MassData(); + aabb = new AABB(); - massData.Mass = density * Settings.Pi * _radius * _radius; - massData.Center = _position; + Vec2 p1 = transform1.Position + Common.Math.Mul(transform1.R, _localPosition); + Vec2 p2 = transform2.Position + Common.Math.Mul(transform2.R, _localPosition); + Vec2 lower = Common.Math.Min(p1, p2); + Vec2 upper = Common.Math.Max(p1, p2); - // inertia about the local origin - massData.I = massData.Mass * (0.5f * _radius * _radius + Vec2.Dot(_position, _position)); - } + aabb.LowerBound.Set(lower.X - _radius, lower.Y - _radius); + aabb.UpperBound.Set(upper.X + _radius, upper.Y + _radius); + } - public override float ComputeSubmergedArea(Vec2 normal, float offset, XForm xf, out Vec2 c) + public override void ComputeMass(out MassData massData) { - Vec2 p = Box2DX.Common.Math.Mul(xf, _position); - float l = -(Vec2.Dot(normal, p) - offset); - if (l < -_radius + Box2DX.Common.Settings.FLT_EPSILON) - { - //Completely dry - c = new Vec2(); - return 0; - } - if (l > _radius) - { - //Completely wet - c = p; - return Box2DX.Common.Settings.Pi * _radius * _radius; - } - - //Magic - float r2 = _radius * _radius; - float l2 = l * l; - float area = r2 * ((float)System.Math.Asin(l / _radius) + Box2DX.Common.Settings.Pi / 2) + - l * Box2DX.Common.Math.Sqrt(r2 - l2); - float com = -2.0f / 3.0f * (float)System.Math.Pow(r2 - l2, 1.5f) / area; - - c.X = p.X + normal.X * com; - c.Y = p.Y + normal.Y * com; + massData = new MassData(); - return area; - } + massData.Mass = _density * Settings.Pi * _radius * _radius; + massData.Center = _localPosition; - /// - /// Get the supporting vertex index in the given direction. - /// - public override int GetSupport(Vec2 d) - { - return 0; + // inertia about the local origin + massData.I = massData.Mass * (0.5f * _radius * _radius + Vec2.Dot(_localPosition, _localPosition)); } /// - /// Get the supporting vertex in the given direction. + /// Get the local position of this circle in its parent body. /// - public override Vec2 GetSupportVertex(Vec2 d) + /// + public Vec2 GetLocalPosition() { - return _position; + return _localPosition; } /// - /// Get a vertex by index. Used by Distance. + /// Get the radius of this circle. /// - public override Vec2 GetVertex(int index) - { - Box2DXDebug.Assert(index == 0); - return _position; - } - - public override float ComputeSweepRadius(Vec2 pivot) + /// + public float GetRadius() { - return Vec2.Distance(_position, pivot); + return _radius; } - - /// - /// Get the vertex count. - /// - public int VertexCount { get { return 1; } } } } \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs b/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs deleted file mode 100644 index 348a293..0000000 --- a/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs +++ /dev/null @@ -1,277 +0,0 @@ -/* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 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. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -using Box2DX.Common; - -namespace Box2DX.Collision -{ - public class EdgeShape : Shape - { - public Vec2 _v1; - public Vec2 _v2; - - public float _length; - - public Vec2 _normal; - - public Vec2 _direction; - - // Unit vector halfway between m_direction and m_prevEdge.m_direction: - public Vec2 _cornerDir1; - - // Unit vector halfway between m_direction and m_nextEdge.m_direction: - public Vec2 _cornerDir2; - - public bool _cornerConvex1; - public bool _cornerConvex2; - - public EdgeShape _nextEdge; - public EdgeShape _prevEdge; - - public EdgeShape() - { - _type = ShapeType.EdgeShape; - _radius = Settings.PolygonRadius; - } - - public override void Dispose() - { - if (_prevEdge != null) - { - _prevEdge._nextEdge = null; - } - - if (_nextEdge != null) - { - _nextEdge._prevEdge = null; - } - } - - public void Set(Vec2 v1, Vec2 v2) - { - _v1 = v1; - _v2 = v2; - - _direction = _v2 - _v1; - _length = _direction.Normalize(); - _normal = Vec2.Cross(_direction, 1.0f); - - _cornerDir1 = _normal; - _cornerDir2 = -1.0f * _normal; - } - - public override bool TestPoint(XForm transform, Vec2 p) - { - return false; - } - - public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda) - { - Vec2 r = segment.P2 - segment.P1; - Vec2 v1 = Common.Math.Mul(transform, _v1); - Vec2 d = Common.Math.Mul(transform, _v2) - v1; - Vec2 n = Vec2.Cross(d, 1.0f); - - float k_slop = 100.0f * Common.Settings.FLT_EPSILON; - float denom = -Vec2.Dot(r, n); - - // Cull back facing collision and ignore parallel segments. - if (denom > k_slop) - { - // Does the segment intersect the infinite line associated with this segment? - Vec2 b = segment.P1 - v1; - float a = Vec2.Dot(b, n); - - if (0.0f <= a && a <= maxLambda * denom) - { - float mu2 = -r.X * b.Y + r.Y * b.X; - - // Does the segment intersect this segment? - if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop)) - { - a /= denom; - n.Normalize(); - lambda = a; - normal = n; - return SegmentCollide.HitCollide; - } - } - } - - lambda = 0; - normal = new Vec2(); - return SegmentCollide.MissCollide; - } - - public override void ComputeAABB(out AABB aabb, XForm transform) - { - Vec2 v1 = Common.Math.Mul(transform, _v1); - Vec2 v2 = Common.Math.Mul(transform, _v2); - - Vec2 r = new Vec2(_radius, _radius); - aabb.LowerBound = Common.Math.Min(v1, v2) - r; - aabb.UpperBound = Common.Math.Max(v1, v2) + r; - } - - public override void ComputeMass(out MassData massData, float density) - { - massData.Mass = 0.0f; - massData.Center = _v1; - massData.I = 0.0f; - } - - public void SetPrevEdge(EdgeShape edge, Vec2 cornerDir, bool convex) - { - _prevEdge = edge; - _cornerDir1 = cornerDir; - _cornerConvex1 = convex; - } - - public void SetNextEdge(EdgeShape edge, Vec2 cornerDir, bool convex) - { - _nextEdge = edge; - _cornerDir2 = cornerDir; - _cornerConvex2 = convex; - } - - public override float ComputeSubmergedArea(Vec2 normal, float offset, XForm xf, out Vec2 c) - { - //Note that v0 is independent of any details of the specific edge - //We are relying on v0 being consistent between multiple edges of the same body - Vec2 v0 = offset * normal; - //b2Vec2 v0 = xf.position + (offset - b2Dot(normal, xf.position)) * normal; - - Vec2 v1 = Common.Math.Mul(xf, _v1); - Vec2 v2 = Common.Math.Mul(xf, _v2); - - float d1 = Vec2.Dot(normal, v1) - offset; - float d2 = Vec2.Dot(normal, v2) - offset; - - if (d1 > 0.0f) - { - if (d2 > 0.0f) - { - c = new Vec2(); - return 0.0f; - } - else - { - v1 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2; - } - } - else - { - if (d2 > 0.0f) - { - v2 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2; - } - else - { - //Nothing - } - } - - // v0,v1,v2 represents a fully submerged triangle - float k_inv3 = 1.0f / 3.0f; - - // Area weighted centroid - c = k_inv3 * (v0 + v1 + v2); - - Vec2 e1 = v1 - v0; - Vec2 e2 = v2 - v0; - - return 0.5f * Vec2.Cross(e1, e2); - } - - public float Length - { - get { return _length; } - } - - public Vec2 Vertex1 - { - get { return _v1; } - } - - public Vec2 Vertex2 - { - get { return _v2; } - } - - public Vec2 NormalVector - { - get { return _normal; } - } - - public Vec2 DirectionVector - { - get { return _direction; } - } - - public Vec2 Corner1Vector - { - get { return _cornerDir1; } - } - - public Vec2 Corner2Vector - { - get { return _cornerDir2; } - } - - public override int GetSupport(Vec2 d) - { - return Vec2.Dot(_v1, d) > Vec2.Dot(_v2, d) ? 0 : 1; - } - - public override Vec2 GetSupportVertex(Vec2 d) - { - return Vec2.Dot(_v1, d) > Vec2.Dot(_v2, d) ? _v1 : _v2; - } - - public override Vec2 GetVertex(int index) - { - Box2DXDebug.Assert(0 <= index && index < 2); - if (index == 0) return _v1; - else return _v2; - } - - public bool Corner1IsConvex - { - get { return _cornerConvex1; } - } - - public bool Corner2IsConvex - { - get { return _cornerConvex2; } - } - - public override float ComputeSweepRadius(Vec2 pivot) - { - float ds1 = Vec2.DistanceSquared(_v1, pivot); - float ds2 = Vec2.DistanceSquared(_v2, pivot); - return Common.Math.Sqrt(Common.Math.Max(ds1, ds2)); - } - } -} diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs.meta b/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs.meta deleted file mode 100644 index 8051fd4..0000000 --- a/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs.meta +++ /dev/null @@ -1,11 +0,0 @@ -fileFormatVersion: 2 -guid: 1e86753e9f98b2d429a099076e61bf32 -MonoImporter: - externalObjects: {} - serializedVersion: 2 - defaultReferences: [] - executionOrder: 0 - icon: {instanceID: 0} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/PolygonShape.cs b/Box2d/Assets/Program/Box2d/Collision/Shapes/PolygonShape.cs index 6a655f7..e68294e 100644 --- a/Box2d/Assets/Program/Box2d/Collision/Shapes/PolygonShape.cs +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/PolygonShape.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -30,144 +30,263 @@ using Box2DX.Common; namespace Box2DX.Collision { /// - /// A convex polygon. It is assumed that the interior of the polygon is to the left of each edge. + /// Convex polygon. The vertices must be in CCW order for a right-handed + /// coordinate system with the z-axis coming out of the screen. /// - public class PolygonShape : Shape + public class PolygonDef : ShapeDef { - internal Vec2 _centroid; - internal Vec2[] _vertices = new Vec2[Settings.MaxPolygonVertices]; - internal Vec2[] _normals = new Vec2[Settings.MaxPolygonVertices]; + /// + /// The number of polygon vertices. + /// + public int VertexCount; - internal int _vertexCount; + /// + /// The polygon vertices in local coordinates. + /// + public Vec2[] Vertices = new Vec2[Settings.MaxPolygonVertices]; - public int VertexCount + public PolygonDef() + { + Type = ShapeType.PolygonShape; + VertexCount = 0; + } + + /// + /// Build vertices to represent an axis-aligned box. + /// + /// The half-width + /// The half-height. + public void SetAsBox(float hx, float hy) { - get { return _vertexCount; } + VertexCount = 4; + Vertices[0].Set(-hx, -hy); + Vertices[1].Set(hx, -hy); + Vertices[2].Set(hx, hy); + Vertices[3].Set(-hx, hy); } - public Vec2[] Vertices + + /// + /// Build vertices to represent an oriented box. + /// + /// The half-width + /// The half-height. + /// The center of the box in local coordinates. + /// The rotation of the box in local coordinates. + public void SetAsBox(float hx, float hy, Vec2 center, float angle) + { + SetAsBox(hx, hy); + XForm xf = new XForm(); + xf.Position = center; + xf.R.Set(angle); + + for (int i = 0; i < VertexCount; ++i) + { + Vertices[i] = Common.Math.Mul(xf, Vertices[i]); + } + } + } + + /// + /// A convex polygon. + /// + public class PolygonShape : Shape, Collision.IGenericShape + { + // Local position of the polygon centroid. + private Vec2 _centroid; + /// + /// Get local centroid relative to the parent body. + /// + /// + //public Vector2 Centroid { get { return _centroid; } } + public Vec2 GetCentroid() { return _centroid; } + + private OBB _obb; + /// + /// Get the oriented bounding box relative to the parent body. + /// + //public OBB OBB { get { return _obb; } } + public OBB GetOBB() { return _obb; } + + private int _vertexCount; + /// + /// Get the vertex count. + /// + public int VertexCount { get { return _vertexCount; } } + + private Vec2[] _vertices = new Vec2[Settings.MaxPolygonVertices]; + /// + /// Get the vertices in local coordinates. + /// + //public Vector2[] Vertices { get { return _vertices; } } + public Vec2[] GetVertices() { return _vertices; } + + private Vec2[] _coreVertices = new Vec2[Settings.MaxPolygonVertices]; + /// + /// Get the core vertices in local coordinates. These vertices + /// represent a smaller polygon that is used for time of impact + /// computations. + /// + //public Vector2[] CoreVertices { get { return _coreVertices; } } + public Vec2[] GetCoreVertices() { return _coreVertices; } + + private Vec2[] _normals = new Vec2[Settings.MaxPolygonVertices]; + /// + /// Get the edge normal vectors. There is one for each vertex. + /// + public Vec2[] Normals { get { return _normals; } } + + /// + /// Get the first vertex and apply the supplied transform. + /// + public Vec2 GetFirstVertex(XForm xf) { - get { return _vertices; } + return Common.Math.Mul(xf, _coreVertices[0]); } /// - /// Copy vertices. This assumes the vertices define a convex polygon. - /// It is assumed that the exterior is the the right of each edge. + /// Get the centroid and apply the supplied transform. + /// + public Vec2 Centroid(XForm xf) + { + return Common.Math.Mul(xf, _centroid); + } + + /// + /// Get the support point in the given world direction. + /// Use the supplied transform. /// - public void Set(Vec2[] vertices, int count) + public Vec2 Support(XForm xf, Vec2 d) { - Box2DXDebug.Assert(3 <= count && count <= Settings.MaxPolygonVertices); - _vertexCount = count; + Vec2 dLocal = Common.Math.MulT(xf.R, d); + + int bestIndex = 0; + float bestValue = Vec2.Dot(_coreVertices[0], dLocal); + for (int i = 1; i < _vertexCount; ++i) + { + float value = Vec2.Dot(_coreVertices[i], dLocal); + if (value > bestValue) + { + bestIndex = i; + bestValue = value; + } + } + + return Common.Math.Mul(xf, _coreVertices[bestIndex]); + } + + internal PolygonShape(ShapeDef def) + : base(def) + { + Box2DXDebug.Assert(def.Type == ShapeType.PolygonShape); + _type = ShapeType.PolygonShape; + PolygonDef poly = (PolygonDef)def; + + // Get the vertices transformed into the body frame. + _vertexCount = poly.VertexCount; + Box2DXDebug.Assert(3 <= _vertexCount && _vertexCount <= Settings.MaxPolygonVertices); - int i; // Copy vertices. - for (i = 0; i < _vertexCount; ++i) + for (int i = 0; i < _vertexCount; ++i) { - _vertices[i] = vertices[i]; + _vertices[i] = poly.Vertices[i]; } // Compute normals. Ensure the edges have non-zero length. - for (i = 0; i < _vertexCount; ++i) + for (int i = 0; i < _vertexCount; ++i) { int i1 = i; - int i2 = i + 1 < count ? i + 1 : 0; + int i2 = i + 1 < _vertexCount ? i + 1 : 0; Vec2 edge = _vertices[i2] - _vertices[i1]; - Box2DXDebug.Assert(edge.LengthSquared() > Settings.FLT_EPSILON_SQUARED); + Box2DXDebug.Assert(edge.LengthSquared() > Common.Settings.FLT_EPSILON * Common.Settings.FLT_EPSILON); _normals[i] = Vec2.Cross(edge, 1.0f); _normals[i].Normalize(); } #if DEBUG - // Ensure the polygon is convex and the interior - // is to the left of each edge. - for (i = 0; i < _vertexCount; ++i) + // Ensure the polygon is convex. + for (int i = 0; i < _vertexCount; ++i) { - int i1 = i; - int i2 = i + 1 < count ? i + 1 : 0; - Vec2 edge = _vertices[i2] - _vertices[i1]; - for (int j = 0; j < _vertexCount; ++j) { // Don't check vertices on the current edge. - if (j == i1 || j == i2) + if (j == i || j == (i + 1) % _vertexCount) { continue; } - Vec2 r = _vertices[j] - _vertices[i1]; - - // Your polygon is non-convex (it has an indentation) or - // has colinear edges. - float s = Vec2.Cross(edge, r); - Box2DXDebug.Assert(s > 0.0f); + // Your polygon is non-convex (it has an indentation). + // Or your polygon is too skinny. + float s = Vec2.Dot(_normals[i], _vertices[j] - _vertices[i]); + Box2DXDebug.Assert(s < -Settings.LinearSlop); } } -#endif - // Compute the polygon centroid. - _centroid = ComputeCentroid(_vertices, _vertexCount); - } + // Ensure the polygon is counter-clockwise. + for (int i = 1; i < _vertexCount; ++i) + { + float cross = Vec2.Cross(_normals[i - 1], _normals[i]); - /// - /// Build vertices to represent an axis-aligned box. - /// - /// The half-width - /// The half-height. - public void SetAsBox(float hx, float hy) - { - _vertexCount = 4; - _vertices[0].Set(-hx, -hy); - _vertices[1].Set(hx, -hy); - _vertices[2].Set(hx, hy); - _vertices[3].Set(-hx, hy); - _normals[0].Set(0.0f, -1.0f); - _normals[1].Set(1.0f, 0.0f); - _normals[2].Set(0.0f, 1.0f); - _normals[3].Set(-1.0f, 0.0f); - _centroid = new Vec2(0); - } + // Keep asinf happy. + cross = Common.Math.Clamp(cross, -1.0f, 1.0f); + // You have consecutive edges that are almost parallel on your polygon. + float angle = (float)System.Math.Asin(cross); + Box2DXDebug.Assert(angle > Settings.AngularSlop); + } +#endif - /// - /// Build vertices to represent an oriented box. - /// - /// The half-width - /// The half-height. - /// The center of the box in local coordinates. - /// The rotation of the box in local coordinates. - public void SetAsBox(float hx, float hy, Vec2 center, float angle) - { - SetAsBox(hx, hy); + // Compute the polygon centroid. + _centroid = ComputeCentroid(poly.Vertices, poly.VertexCount); - XForm xf = new XForm(); - xf.Position = center; - xf.R.Set(angle); + // Compute the oriented bounding box. + ComputeOBB(out _obb, _vertices, _vertexCount); - // Transform vertices and normals. + // Create core polygon shape by shifting edges inward. + // Also compute the min/max radius for CCD. for (int i = 0; i < _vertexCount; ++i) { - _vertices[i] = Common.Math.Mul(xf, _vertices[i]); - _normals[i] = Common.Math.Mul(xf.R, _normals[i]); + int i1 = i - 1 >= 0 ? i - 1 : _vertexCount - 1; + int i2 = i; + + Vec2 n1 = _normals[i1]; + Vec2 n2 = _normals[i2]; + Vec2 v = _vertices[i] - _centroid; ; + + Vec2 d = new Vec2(); + d.X = Vec2.Dot(n1, v) - Settings.ToiSlop; + d.Y = Vec2.Dot(n2, v) - Settings.ToiSlop; + + // Shifting the edge inward by b2_toiSlop should + // not cause the plane to pass the centroid. + + // Your shape has a radius/extent less than b2_toiSlop. + Box2DXDebug.Assert(d.X >= 0.0f); + Box2DXDebug.Assert(d.Y >= 0.0f); + Mat22 A = new Mat22(); + A.Col1.X = n1.X; A.Col2.X = n1.Y; + A.Col1.Y = n2.X; A.Col2.Y = n2.Y; + _coreVertices[i] = A.Solve(d) + _centroid; } } - public void SetAsEdge(Vec2 v1, Vec2 v2) + internal override void UpdateSweepRadius(Vec2 center) { - _vertexCount = 2; - _vertices[0] = v1; - _vertices[1] = v2; - _centroid = 0.5f * (v1 + v2); - _normals[0] = Vec2.Cross(v2 - v1, 1.0f); - _normals[0].Normalize(); - _normals[1] = -_normals[0]; - } + // Update the sweep radius (maximum radius) as measured from + // a local center point. + _sweepRadius = 0.0f; + for (int i = 0; i < _vertexCount; ++i) + { + Vec2 d = _coreVertices[i] - center; + _sweepRadius = Common.Math.Max(_sweepRadius, d.Length()); + } + } public override bool TestPoint(XForm xf, Vec2 p) { Vec2 pLocal = Common.Math.MulT(xf.R, p - xf.Position); - int vc = _vertexCount; - for (int i = 0; i < vc; ++i) + for (int i = 0; i < _vertexCount; ++i) { float dot = Vec2.Dot(_normals[i], pLocal - _vertices[i]); if (dot > 0.0f) @@ -248,22 +367,24 @@ namespace Box2DX.Collision public override void ComputeAABB(out AABB aabb, XForm xf) { - Vec2 lower = Common.Math.Mul(xf, _vertices[0]); - Vec2 upper = lower; - - for (int i = 1; i < _vertexCount; ++i) - { - Vec2 v = Common.Math.Mul(xf, _vertices[i]); - lower = Common.Math.Min(lower, v); - upper = Common.Math.Max(upper, v); - } + Mat22 R = Common.Math.Mul(xf.R, _obb.R); + Mat22 absR = Common.Math.Abs(R); + Vec2 h = Common.Math.Mul(absR, _obb.Extents); + Vec2 position = xf.Position + Common.Math.Mul(xf.R, _obb.Center); + aabb.LowerBound = position - h; + aabb.UpperBound = position + h; + } - Vec2 r = new Vec2(_radius); - aabb.LowerBound = lower - r; - aabb.UpperBound = upper + r; + public override void ComputeSweptAABB(out AABB aabb, XForm transform1, XForm transform2) + { + AABB aabb1, aabb2; + ComputeAABB(out aabb1, transform1); + ComputeAABB(out aabb2, transform2); + aabb.LowerBound = Common.Math.Min(aabb1.LowerBound, aabb2.LowerBound); + aabb.UpperBound = Common.Math.Max(aabb1.UpperBound, aabb2.UpperBound); } - public override void ComputeMass(out MassData massData, float denstity) + public override void ComputeMass(out MassData massData) { // Polygon mass, centroid, and inertia. // Let rho be the polygon density in mass per unit area. @@ -291,24 +412,25 @@ namespace Box2DX.Collision Box2DXDebug.Assert(_vertexCount >= 3); - Vec2 center = new Vec2(0); + Vec2 center = new Vec2(); + center.Set(0.0f, 0.0f); float area = 0.0f; float I = 0.0f; // pRef is the reference point for forming triangles. // It's location doesn't change the result (except for rounding error). - Vec2 pRef = new Vec2(0); + Vec2 pRef = new Vec2(0.0f, 0.0f); #if O // This code would put the reference point inside the polygon. - for (int i = 0; i < vCount; ++i) + for (int i = 0; i < _vertexCount; ++i) { pRef += _vertices[i]; } pRef *= 1.0f / count; #endif - const float k_inv3 = 1.0f / 3.0f; + float k_inv3 = 1.0f / 3.0f; for (int i = 0; i < _vertexCount; ++i) { @@ -339,7 +461,7 @@ namespace Box2DX.Collision } // Total mass - massData.Mass = denstity * area; + massData.Mass = _density * area; // Center of mass Box2DXDebug.Assert(area > Common.Settings.FLT_EPSILON); @@ -347,197 +469,19 @@ namespace Box2DX.Collision massData.Center = center; // Inertia tensor relative to the local origin. - massData.I = denstity * I; - } - - public override float ComputeSubmergedArea(Vec2 normal, float offset, XForm xf, out Vec2 c) - { - //Transform plane into shape co-ordinates - Vec2 normalL = Box2DX.Common.Math.MulT(xf.R, normal); - float offsetL = offset - Vec2.Dot(normal, xf.Position); - - float[] depths = new float[Common.Settings.MaxPolygonVertices]; - int diveCount = 0; - int intoIndex = -1; - int outoIndex = -1; - - bool lastSubmerged = false; - int i; - for (i = 0; i < _vertexCount; i++) - { - depths[i] = Vec2.Dot(normalL, _vertices[i]) - offsetL; - bool isSubmerged = depths[i] < -Common.Settings.FLT_EPSILON; - if (i > 0) - { - if (isSubmerged) - { - if (!lastSubmerged) - { - intoIndex = i - 1; - diveCount++; - } - } - else - { - if (lastSubmerged) - { - outoIndex = i - 1; - diveCount++; - } - } - } - lastSubmerged = isSubmerged; - } - switch (diveCount) - { - case 0: - if (lastSubmerged) - { - //Completely submerged - MassData md; - ComputeMass(out md, 1f); - c = Common.Math.Mul(xf, md.Center); - return md.Mass; - } - else - { - //Completely dry - c = new Vec2(); - return 0; - } - break; - case 1: - if (intoIndex == -1) - { - intoIndex = _vertexCount - 1; - } - else - { - outoIndex = _vertexCount - 1; - } - break; - } - int intoIndex2 = (intoIndex + 1) % _vertexCount; - int outoIndex2 = (outoIndex + 1) % _vertexCount; - - float intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]); - float outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]); - - Vec2 intoVec = new Vec2(_vertices[intoIndex].X * (1 - intoLambda) + _vertices[intoIndex2].X * intoLambda, - _vertices[intoIndex].Y * (1 - intoLambda) + _vertices[intoIndex2].Y * intoLambda); - Vec2 outoVec = new Vec2(_vertices[outoIndex].X * (1 - outoLambda) + _vertices[outoIndex2].X * outoLambda, - _vertices[outoIndex].Y * (1 - outoLambda) + _vertices[outoIndex2].Y * outoLambda); - - //Initialize accumulator - float area = 0; - Vec2 center = new Vec2(0); - Vec2 p2 = _vertices[intoIndex2]; - Vec2 p3; - - const float k_inv3 = 1.0f / 3.0f; - - //An awkward loop from intoIndex2+1 to outIndex2 - i = intoIndex2; - while (i != outoIndex2) - { - i = (i + 1) % _vertexCount; - if (i == outoIndex2) - p3 = outoVec; - else - p3 = _vertices[i]; - //Add the triangle formed by intoVec,p2,p3 - { - Vec2 e1 = p2 - intoVec; - Vec2 e2 = p3 - intoVec; - - float D = Vec2.Cross(e1, e2); - - float triangleArea = 0.5f * D; - - area += triangleArea; - - // Area weighted centroid - center += triangleArea * k_inv3 * (intoVec + p2 + p3); - - } - // - p2 = p3; - } - - //Normalize and transform centroid - center *= 1.0f / area; - - c = Common.Math.Mul(xf, center); - - return area; - } - - public override float ComputeSweepRadius(Vec2 pivot) - { - int vCount = _vertexCount; - Box2DXDebug.Assert(vCount > 0); - float sr = Vec2.DistanceSquared(_vertices[0], pivot); - for (int i = 1; i < vCount; ++i) - { - sr = Common.Math.Max(sr, Vec2.DistanceSquared(_vertices[i], pivot)); - } - - return Common.Math.Sqrt(sr); - } - - /// - /// Get the supporting vertex index in the given direction. - /// - public override int GetSupport(Vec2 d) - { - int bestIndex = 0; - float bestValue = Vec2.Dot(_vertices[0], d); - for (int i = 1; i < _vertexCount; ++i) - { - float value = Vec2.Dot(_vertices[i], d); - if (value > bestValue) - { - bestIndex = i; - bestValue = value; - } - } - - return bestIndex; - } - - public override Vec2 GetSupportVertex(Vec2 d) - { - int bestIndex = 0; - float bestValue = Vec2.Dot(_vertices[0], d); - for (int i = 1; i < _vertexCount; ++i) - { - float value = Vec2.Dot(_vertices[i], d); - if (value > bestValue) - { - bestIndex = i; - bestValue = value; - } - } - - return _vertices[bestIndex]; - } - - public override Vec2 GetVertex(int index) - { - Box2DXDebug.Assert(0 <= index && index < _vertexCount); - return _vertices[index]; + massData.I = _density * I; } public static Vec2 ComputeCentroid(Vec2[] vs, int count) { Box2DXDebug.Assert(count >= 3); - Vec2 c = new Vec2(0f); - float area = 0f; + Vec2 c = new Vec2(); c.Set(0.0f, 0.0f); + float area = 0.0f; // pRef is the reference point for forming triangles. // It's location doesn't change the result (except for rounding error). - Vec2 pRef = new Vec2(0f); + Vec2 pRef = new Vec2(0.0f, 0.0f); #if O // This code would put the reference point inside the polygon. for (int i = 0; i < count; ++i) @@ -547,7 +491,7 @@ namespace Box2DX.Collision pRef *= 1.0f / count; #endif - const float inv3 = 1.0f / 3.0f; + float inv3 = 1.0f / 3.0f; for (int i = 0; i < count; ++i) { @@ -574,104 +518,7 @@ namespace Box2DX.Collision return c; } - public PolygonShape() - { - _type = ShapeType.PolygonShape; - _radius = Settings.PolygonRadius; - - /*Box2DXDebug.Assert(def.Type == ShapeType.PolygonShape); - _type = ShapeType.PolygonShape; - PolygonDef poly = (PolygonDef)def; - - // Get the vertices transformed into the body frame. - _vertexCount = poly.VertexCount; - Box2DXDebug.Assert(3 <= _vertexCount && _vertexCount <= Settings.MaxPolygonVertices); - - // Copy vertices. - for (int i = 0; i < _vertexCount; ++i) - { - _vertices[i] = poly.Vertices[i]; - } - - // Compute normals. Ensure the edges have non-zero length. - for (int i = 0; i < _vertexCount; ++i) - { - int i1 = i; - int i2 = i + 1 < _vertexCount ? i + 1 : 0; - Vec2 edge = _vertices[i2] - _vertices[i1]; - Box2DXDebug.Assert(edge.LengthSquared() > Common.Settings.FLT_EPSILON * Common.Settings.FLT_EPSILON); - _normals[i] = Vec2.Cross(edge, 1.0f); - _normals[i].Normalize(); - } - -#if DEBUG - // Ensure the polygon is convex. - for (int i = 0; i < _vertexCount; ++i) - { - for (int j = 0; j < _vertexCount; ++j) - { - // Don't check vertices on the current edge. - if (j == i || j == (i + 1) % _vertexCount) - { - continue; - } - - // Your polygon is non-convex (it has an indentation). - // Or your polygon is too skinny. - float s = Vec2.Dot(_normals[i], _vertices[j] - _vertices[i]); - Box2DXDebug.Assert(s < -Settings.LinearSlop); - } - } - - // Ensure the polygon is counter-clockwise. - for (int i = 1; i < _vertexCount; ++i) - { - float cross = Vec2.Cross(_normals[i - 1], _normals[i]); - - // Keep asinf happy. - cross = Common.Math.Clamp(cross, -1.0f, 1.0f); - - // You have consecutive edges that are almost parallel on your polygon. - float angle = (float)System.Math.Asin(cross); - Box2DXDebug.Assert(angle > Settings.AngularSlop); - } -#endif - - // Compute the polygon centroid. - _centroid = ComputeCentroid(poly.Vertices, poly.VertexCount); - - // Compute the oriented bounding box. - ComputeOBB(out _obb, _vertices, _vertexCount); - - // Create core polygon shape by shifting edges inward. - // Also compute the min/max radius for CCD. - for (int i = 0; i < _vertexCount; ++i) - { - int i1 = i - 1 >= 0 ? i - 1 : _vertexCount - 1; - int i2 = i; - - Vec2 n1 = _normals[i1]; - Vec2 n2 = _normals[i2]; - Vec2 v = _vertices[i] - _centroid; ; - - Vec2 d = new Vec2(); - d.X = Vec2.Dot(n1, v) - Settings.ToiSlop; - d.Y = Vec2.Dot(n2, v) - Settings.ToiSlop; - - // Shifting the edge inward by b2_toiSlop should - // not cause the plane to pass the centroid. - - // Your shape has a radius/extent less than b2_toiSlop. - Box2DXDebug.Assert(d.X >= 0.0f); - Box2DXDebug.Assert(d.Y >= 0.0f); - Mat22 A = new Mat22(); - A.Col1.X = n1.X; A.Col2.X = n1.Y; - A.Col1.Y = n2.X; A.Col2.Y = n2.Y; - _coreVertices[i] = A.Solve(d) + _centroid; - }*/ - } - - /*// http://www.geometrictools.com/Documentation/MinimumAreaRectangle.pdf + // http://www.geometrictools.com/Documentation/MinimumAreaRectangle.pdf public static void ComputeOBB(out OBB obb, Vec2[] vs, int count) { obb = new OBB(); @@ -719,6 +566,6 @@ namespace Box2DX.Collision } Box2DXDebug.Assert(minArea < Common.Settings.FLT_MAX); - }*/ + } } } \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs b/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs index 1afd116..94480ee 100644 --- a/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -20,7 +20,11 @@ */ using System; +using System.Collections.Generic; +using System.Text; + using Box2DX.Common; +using Box2DX.Dynamics; namespace Box2DX.Collision { @@ -45,6 +49,30 @@ namespace Box2DX.Collision public float I; } + /// + /// This holds contact filtering data. + /// + public struct FilterData + { + /// + /// The collision category bits. Normally you would just set one bit. + /// + public ushort CategoryBits; + + /// + /// The collision mask bits. This states the categories that this + /// shape would accept for collision. + /// + public ushort MaskBits; + + /// + /// Collision groups allow a certain group of objects to never collide (negative) + /// or always collide (positive). Zero means no collision group. Non-zero group + /// filtering always wins against the mask bits. + /// + public short GroupIndex; + } + /// /// The various collision shape types supported by Box2D. /// @@ -53,7 +81,6 @@ namespace Box2DX.Collision UnknownShape = -1, CircleShape, PolygonShape, - EdgeShape, ShapeTypeCount, } @@ -67,20 +94,156 @@ namespace Box2DX.Collision HitCollide = 1 } +#warning "CAS" + /// + /// A shape definition is used to construct a shape. This class defines an + /// abstract shape definition. You can reuse shape definitions safely. + /// + public class ShapeDef + { + /// + /// Holds the shape type for down-casting. + /// + public ShapeType Type; + + /// + /// Use this to store application specify shape data. + /// + public object UserData; + + /// + /// The shape's friction coefficient, usually in the range [0,1]. + /// + public float Friction; + + /// + /// The shape's restitution (elasticity) usually in the range [0,1]. + /// + public float Restitution; + + /// + /// The shape's density, usually in kg/m^2. + /// + public float Density; + + /// + /// A sensor shape collects contact information but never generates a collision + /// response. + /// + public bool IsSensor; + + /// + /// Contact filtering data. + /// + public FilterData Filter; + + /// + /// The constructor sets the default shape definition values. + /// + public ShapeDef() + { + Type = ShapeType.UnknownShape; + UserData = null; + Friction = 0.2f; + Restitution = 0.0f; + Density = 0.0f; + Filter.CategoryBits = 0x0001; + Filter.MaskBits = 0xFFFF; + Filter.GroupIndex = 0; + IsSensor = false; + } + } + /// - /// A shape is used for collision detection. You can create a shape however you like. - /// Shapes used for simulation in World are created automatically when a Fixture is created. + /// A shape is used for collision detection. Shapes are created in World. + /// You can use shape for collision detection before they are attached to the world. + /// Warning: you cannot reuse shapes. /// public abstract class Shape : IDisposable { - #region Fields + #region Fields and Properties - protected ShapeType _type = ShapeType.UnknownShape; - internal float _radius; + protected ShapeType _type; + /// + /// Get the type of this shape. You can use this to down cast to the concrete shape. + /// + //public ShapeType Type { get { return _type; } } + public new ShapeType GetType() { return _type; } - #endregion Fields + internal Shape _next; + /// + /// Get the next shape in the parent body's shape list. + /// + //public Shape Next { get { return _next; } } + public Shape GetNext() { return _next; } - protected Shape() { } + internal Body _body; + /// + /// Get the parent body of this shape. This is NULL if the shape is not attached. + /// + //public Body Body { get { return _body; } } + public Body GetBody() { return _body; } + + // Sweep radius relative to the parent body's center of mass. + protected float _sweepRadius; + /// + /// Get the maximum radius about the parent body's center of mass. + /// + public float GetSweepRadius() { return _sweepRadius; } + + protected float _density; + + protected float _friction; + public float Friction { get { return _friction; } set { _friction = value; } } + + protected float _restitution; + public float Restitution { get { return _restitution; } set { _restitution = value; } } + + protected ushort _proxyId; + + protected bool _isSensor; + /// + /// Is this shape a sensor (non-solid)? + /// + public bool IsSensor { get { return _isSensor; } } + + protected FilterData _filter; + /// + /// Get\Set the contact filtering data. You must call World.Refilter to correct + /// existing contacts/non-contacts. + /// + public FilterData FilterData + { + get { return _filter; } + set { _filter = value; } + } + + protected object _userData; + /// + /// Get the user data that was assigned in the shape definition. Use this to + /// store your application specific data. + /// + public object UserData + { + get { return _userData; } + set { _userData = value; } + } + + #endregion Fields and Properties + + protected Shape(ShapeDef def) + { + _userData = def.UserData; + _friction = def.Friction; + _restitution = def.Restitution; + _density = def.Density; + _body = null; + _sweepRadius = 0.0f; + _next = null; + _proxyId = PairManager.NullProxy; + _filter = def.Filter; + _isSensor = def.IsSensor; + } /// /// Test a point for containment in this shape. This only works for convex shapes. @@ -109,36 +272,145 @@ namespace Box2DX.Collision /// The world transform of the shape. public abstract void ComputeAABB(out AABB aabb, XForm xf); + /// + /// Given two transforms, compute the associated swept axis aligned bounding box for this shape. + /// + /// Returns the axis aligned box. + /// The starting shape world transform. + /// The ending shape world transform. + public abstract void ComputeSweptAABB(out AABB aabb, XForm xf1, XForm xf2); + /// /// Compute the mass properties of this shape using its dimensions and density. /// The inertia tensor is computed about the local origin, not the centroid. /// /// Returns the mass data for this shape - public abstract void ComputeMass(out MassData massData, float density); + public abstract void ComputeMass(out MassData massData); - /// - /// Compute the volume and centroid of this shape intersected with a half plane. - /// - /// Normal the surface normal. - /// Offset the surface offset along normal. - /// The shape transform. - /// Returns the centroid. - /// The total volume less than offset along normal. - public abstract float ComputeSubmergedArea(Vec2 normal, float offset, XForm xf, out Vec2 c); + internal abstract void UpdateSweepRadius(Vec2 center); - /// - /// Compute the sweep radius. This is used for conservative advancement (continuous collision detection). - /// - /// Pivot is the pivot point for rotation. - /// The distance of the furthest point from the pivot. - public abstract float ComputeSweepRadius(Vec2 pivot); + internal static Shape Create(ShapeDef def) + { + switch (def.Type) + { + case ShapeType.CircleShape: + { + return new CircleShape(def); + } + + case ShapeType.PolygonShape: + { + return new PolygonShape(def); + } + + default: + Box2DXDebug.Assert(false); + return null; + } + } + + internal static void Destroy(Shape s) + { + switch (s.GetType()) + { + case ShapeType.CircleShape: + if (s is IDisposable) + (s as IDisposable).Dispose(); + s = null; + break; + + case ShapeType.PolygonShape: + if (s is IDisposable) + (s as IDisposable).Dispose(); + s = null; + break; + + default: + Box2DXDebug.Assert(false); + break; + } + } + + internal void CreateProxy(BroadPhase broadPhase, XForm transform) + { + Box2DXDebug.Assert(_proxyId == PairManager.NullProxy); - public abstract Vec2 GetVertex(int index); + AABB aabb; + ComputeAABB(out aabb, transform); - public abstract int GetSupport(Vec2 d); + bool inRange = broadPhase.InRange(aabb); - public abstract Vec2 GetSupportVertex(Vec2 d); + // You are creating a shape outside the world box. + Box2DXDebug.Assert(inRange); - public virtual void Dispose(){} - } + if (inRange) + { + _proxyId = broadPhase.CreateProxy(aabb, this); + } + else + { + _proxyId = PairManager.NullProxy; + } + } + + internal void DestroyProxy(BroadPhase broadPhase) + { + if (_proxyId != PairManager.NullProxy) + { + broadPhase.DestroyProxy(_proxyId); + _proxyId = PairManager.NullProxy; + } + } + + internal bool Synchronize(BroadPhase broadPhase, XForm transform1, XForm transform2) + { + if (_proxyId == PairManager.NullProxy) + { + return false; + } + + // Compute an AABB that covers the swept shape (may miss some rotation effect). + AABB aabb; + ComputeSweptAABB(out aabb, transform1, transform2); + + if (broadPhase.InRange(aabb)) + { + broadPhase.MoveProxy(_proxyId, aabb); + return true; + } + else + { + return false; + } + } + + internal void RefilterProxy(BroadPhase broadPhase, XForm transform) + { + if (_proxyId == PairManager.NullProxy) + { + return; + } + + broadPhase.DestroyProxy(_proxyId); + + AABB aabb; + ComputeAABB(out aabb, transform); + + bool inRange = broadPhase.InRange(aabb); + + if (inRange) + { + _proxyId = broadPhase.CreateProxy(aabb, this); + } + else + { + _proxyId = PairManager.NullProxy; + } + } + + public virtual void Dispose() + { + Box2DXDebug.Assert(_proxyId == PairManager.NullProxy); + } + } } \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/TODO.txt b/Box2d/Assets/Program/Box2d/Collision/TODO.txt deleted file mode 100644 index 312bdc2..0000000 --- a/Box2d/Assets/Program/Box2d/Collision/TODO.txt +++ /dev/null @@ -1 +0,0 @@ -check broadphase, port dynamictree \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/TODO.txt.meta b/Box2d/Assets/Program/Box2d/Collision/TODO.txt.meta deleted file mode 100644 index 3719ec6..0000000 --- a/Box2d/Assets/Program/Box2d/Collision/TODO.txt.meta +++ /dev/null @@ -1,7 +0,0 @@ -fileFormatVersion: 2 -guid: 18843330e42568c4bbff292ac904fd78 -TextScriptImporter: - externalObjects: {} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Common/Mat22.cs b/Box2d/Assets/Program/Box2d/Common/Mat22.cs index ba70149..5550ab6 100644 --- a/Box2d/Assets/Program/Box2d/Common/Mat22.cs +++ b/Box2d/Assets/Program/Box2d/Common/Mat22.cs @@ -110,7 +110,7 @@ namespace Box2DX.Common /// /// Compute the inverse of this matrix, such that inv(A) * A = identity. /// - public Mat22 GetInverse() + public Mat22 Invert() { float a = Col1.X, b = Col2.X, c = Col1.Y, d = Col2.Y; Mat22 B = new Mat22(); diff --git a/Box2d/Assets/Program/Box2d/Common/Math.cs b/Box2d/Assets/Program/Box2d/Common/Math.cs index 8c35e3a..ea3b037 100644 --- a/Box2d/Assets/Program/Box2d/Common/Math.cs +++ b/Box2d/Assets/Program/Box2d/Common/Math.cs @@ -27,6 +27,10 @@ namespace Box2DX.Common { public class Math { + //public static float FLT_EPSILON = 1.192092896e-07f; //smallest such that 1.0f+FLT_EPSILON != 1.0f + public static readonly float FLOAT32_EPSILON = 1.192092896e-07f; + //public static float FLT_MAX = 3.402823466e+38F; + public static readonly float FLOAT32_MAX = 3.402823466e+38F; public static readonly ushort USHRT_MAX = 0xffff; public static readonly byte UCHAR_MAX = 0xff; public static readonly int RAND_LIMIT = 32767; @@ -256,10 +260,5 @@ namespace Box2DX.Common Vec3 u = v.X * A.Col1 + v.Y * A.Col2 + v.Z * A.Col3; return u; } - - public static float Atan2(float y, float x) - { - return (float)System.Math.Atan2(y, x); - } } } diff --git a/Box2d/Assets/Program/Box2d/Common/Settings.cs b/Box2d/Assets/Program/Box2d/Common/Settings.cs index d9184ad..e3e971f 100644 --- a/Box2d/Assets/Program/Box2d/Common/Settings.cs +++ b/Box2d/Assets/Program/Box2d/Common/Settings.cs @@ -33,8 +33,7 @@ namespace Box2DX.Common public static float FORCE_SCALE2(x){ return x<<7;} public static float FORCE_INV_SCALE2(x) {return x>>7;} #else - public static readonly float FLT_EPSILON = 1.192092896e-07F;//smallest such that 1.0f+FLT_EPSILON != 1.0f - public static readonly float FLT_EPSILON_SQUARED = FLT_EPSILON * FLT_EPSILON;//smallest such that 1.0f+FLT_EPSILON != 1.0f + public static readonly float FLT_EPSILON = 1.192092896e-07F; public static readonly float FLT_MAX = 3.402823466e+38F; public static float FORCE_SCALE(float x) { return x; } public static float FORCE_INV_SCALE(float x) { return x; } @@ -64,13 +63,6 @@ namespace Box2DX.Common /// public static readonly float AngularSlop = 2.0f / 180.0f * Pi; // 2 degrees - /// - /// The radius of the polygon/edge shape skin. This should not be modified. Making - /// this smaller means polygons will have and insufficient for continuous collision. - /// Making it larger may create artifacts for vertex collision. - /// - public static readonly float PolygonRadius = 2.0f * LinearSlop; - /// /// Continuous collision detection (CCD) works with core, shrunken shapes. This is amount /// by which shapes are automatically shrunk to work with CCD. @@ -124,21 +116,6 @@ namespace Box2DX.Common #if !TARGET_FLOAT32_IS_FIXED public static readonly float MaxAngularVelocitySquared = MaxAngularVelocity * MaxAngularVelocity; #endif - - /// - /// The maximum linear velocity of a body. This limit is very large and is used - /// to prevent numerical problems. You shouldn't need to adjust this. - /// - public static readonly float MaxTranslation = 2.0f; - public static readonly float MaxTranslationSquared = (MaxTranslation * MaxTranslation); - - /// - /// The maximum angular velocity of a body. This limit is very large and is used - /// to prevent numerical problems. You shouldn't need to adjust this. - /// - public static readonly float MaxRotation = (0.5f * Pi); - public static readonly float MaxRotationSquared = (MaxRotation * MaxRotation); - /// /// 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. diff --git a/Box2d/Assets/Program/Box2d/Common/Sweep.cs b/Box2d/Assets/Program/Box2d/Common/Sweep.cs index 09e9560..9d967e5 100644 --- a/Box2d/Assets/Program/Box2d/Common/Sweep.cs +++ b/Box2d/Assets/Program/Box2d/Common/Sweep.cs @@ -37,16 +37,27 @@ namespace Box2DX.Common /// /// Get the interpolated transform at a specific time. /// - /// Alpha is a factor in [0,1], where 0 indicates t0. - public void GetTransform(out XForm xf, float alpha) + /// The normalized time in [0,1]. + public void GetXForm(out XForm xf, float t) { xf = new XForm(); - xf.Position = (1.0f - alpha) * C0 + alpha * C; - float angle = (1.0f - alpha) * A0 + alpha * A; - xf.R.Set(angle); + + // center = p + R * LocalCenter + if (1.0f - T0 > Math.FLOAT32_EPSILON) + { + float alpha = (t - T0) / (1.0f - T0); + xf.Position = (1.0f - alpha) * C0 + alpha * C; + float angle = (1.0f - alpha) * A0 + alpha * A; + xf.R.Set(angle); + } + else + { + xf.Position = C; + xf.R.Set(A); + } // Shift to origin - xf.Position -= Common.Math.Mul(xf.R, LocalCenter); + xf.Position -= Math.Mul(xf.R, LocalCenter); } /// @@ -55,7 +66,7 @@ namespace Box2DX.Common /// The new initial time. public void Advance(float t) { - if (T0 < t && 1.0f - T0 > Settings.FLT_EPSILON) + if (T0 < t && 1.0f - T0 > Math.FLOAT32_EPSILON) { float alpha = (t - T0) / (1.0f - T0); C0 = (1.0f - alpha) * C0 + alpha * C; diff --git a/Box2d/Assets/Program/Box2d/Common/Vec2.cs b/Box2d/Assets/Program/Box2d/Common/Vec2.cs index d614845..656e1be 100644 --- a/Box2d/Assets/Program/Box2d/Common/Vec2.cs +++ b/Box2d/Assets/Program/Box2d/Common/Vec2.cs @@ -32,38 +32,6 @@ namespace Box2DX.Common { public float X, Y; - public float this[int i] - { - get - { - if (i == 0) return X; - else if (i == 1) return Y; - else - { - Box2DXDebug.Assert(false, "Incorrect Vec2 element!"); - return 0; - } - } - set - { - if (i == 0) X = value; - else if (i == 1) Y = value; - else - { - Box2DXDebug.Assert(false, "Incorrect Vec2 element!"); - } - } - } - - /// - /// Construct using coordinates. - /// - public Vec2(float x) - { - X = x; - Y = x; - } - /// /// Construct using coordinates. /// @@ -83,8 +51,6 @@ namespace Box2DX.Common /// public void Set(float x, float y) { X = x; Y = y; } - public void Set(float xy) { X = xy; Y = xy; } - /// /// Get the length of this vector (the norm). /// @@ -108,7 +74,7 @@ namespace Box2DX.Common public float Normalize() { float length = Length(); - if (length < Settings.FLT_EPSILON) + if (length < Math.FLOAT32_EPSILON) { return 0.0f; } @@ -172,7 +138,7 @@ namespace Box2DX.Common public static bool operator !=(Vec2 a, Vec2 b) { - return a.X != b.X || a.Y != b.Y; + return a.X != b.X && a.Y != b.Y; } public static Vec2 Zero { get { return new Vec2(0, 0); } } diff --git a/Box2d/Assets/Program/Box2d/Common/XForm.cs b/Box2d/Assets/Program/Box2d/Common/XForm.cs index de507b5..2664047 100644 --- a/Box2d/Assets/Program/Box2d/Common/XForm.cs +++ b/Box2d/Assets/Program/Box2d/Common/XForm.cs @@ -54,19 +54,6 @@ namespace Box2DX.Common R.SetIdentity(); } - /// Set this based on the position and angle. - public void Set(Vec2 p, float angle) - { - Position = p; - R.Set(angle); - } - - /// Calculate the angle that the rotation matrix represents. - public float GetAngle() - { - return Math.Atan2(R.Col1.Y, R.Col1.X); - } - public static XForm Identity { get { return new XForm(Vec2.Zero, Mat22.Identity); } } } } diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Body.cs b/Box2d/Assets/Program/Box2d/Dynamics/Body.cs index 1daa9a5..f7dd434 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Body.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Body.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -20,6 +20,8 @@ */ using System; +using System.Collections.Generic; +using System.Text; using Box2DX.Common; using Box2DX.Collision; @@ -30,12 +32,12 @@ namespace Box2DX.Dynamics /// A body definition holds all the data needed to construct a rigid body. /// You can safely re-use body definitions. /// - public struct BodyDef + public class BodyDef { /// /// This constructor sets the body definition default values. /// - public BodyDef(byte init) + public BodyDef() { MassData = new MassData(); MassData.Center.SetZero(); @@ -45,8 +47,6 @@ namespace Box2DX.Dynamics Position = new Vec2(); Position.Set(0.0f, 0.0f); Angle = 0.0f; - LinearVelocity = new Vec2(0f, 0f); - AngularVelocity = 0.0f; LinearDamping = 0.0f; AngularDamping = 0.0f; AllowSleep = true; @@ -78,12 +78,6 @@ namespace Box2DX.Dynamics /// public float Angle; - /// The linear velocity of the body in world co-ordinates. - public Vec2 LinearVelocity; - - // The angular velocity of the body. - public float AngularVelocity; - /// /// Linear damping is use to reduce the linear velocity. The damping parameter /// can be larger than 1.0f but the damping effect becomes sensitive to the @@ -124,10 +118,11 @@ namespace Box2DX.Dynamics } /// - /// A rigid body. These are created via World.CreateBody. + /// A rigid body. /// public class Body : IDisposable { + [Flags] public enum BodyFlags { @@ -151,7 +146,7 @@ namespace Box2DX.Dynamics internal int _islandIndex; - internal XForm _xf; // the body origin transform + private XForm _xf; // the body origin transform internal Sweep _sweep; // the swept motion for CCD @@ -165,17 +160,15 @@ namespace Box2DX.Dynamics internal Body _prev; internal Body _next; - internal Fixture _fixtureList; - internal int _fixtureCount; + internal Shape _shapeList; + internal int _shapeCount; internal JointEdge _jointList; internal ContactEdge _contactList; - internal Controllers.ControllerEdge _controllerList; - internal float _mass; internal float _invMass; - internal float _I; + private float _I; internal float _invI; internal float _linearDamping; @@ -218,29 +211,25 @@ namespace Box2DX.Dynamics _sweep.A0 = _sweep.A = bd.Angle; _sweep.C0 = _sweep.C = Common.Math.Mul(_xf, _sweep.LocalCenter); - //_jointList = null; - //_contactList = null; - //_controllerList = null; - //_prev = null; - //_next = null; - - _linearVelocity = bd.LinearVelocity; - _angularVelocity = bd.AngularVelocity; + _jointList = null; + _contactList = null; + _prev = null; + _next = null; _linearDamping = bd.LinearDamping; _angularDamping = bd.AngularDamping; - //_force.Set(0.0f, 0.0f); - //_torque = 0.0f; + _force.Set(0.0f, 0.0f); + _torque = 0.0f; - //_linearVelocity.SetZero(); - //_angularVelocity = 0.0f; + _linearVelocity.SetZero(); + _angularVelocity = 0.0f; - //_sleepTime = 0.0f; + _sleepTime = 0.0f; - //_invMass = 0.0f; - //_I = 0.0f; - //_invI = 0.0f; + _invMass = 0.0f; + _I = 0.0f; + _invI = 0.0f; _mass = bd.MassData.Mass; @@ -249,9 +238,12 @@ namespace Box2DX.Dynamics _invMass = 1.0f / _mass; } - _I = bd.MassData.I; + if ((_flags & BodyFlags.FixedRotation) == 0) + { + _I = bd.MassData.I; + } - if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0) + if (_I > 0.0f) { _invI = 1.0f / _I; } @@ -267,8 +259,8 @@ namespace Box2DX.Dynamics _userData = bd.UserData; - //_fixtureList = null; - //_fixtureCount = 0; + _shapeList = null; + _shapeCount = 0; } public void Dispose() @@ -277,55 +269,13 @@ namespace Box2DX.Dynamics // shapes and joints are destroyed in World.Destroy } - internal bool SynchronizeFixtures() - { - XForm xf1 = new XForm(); - xf1.R.Set(_sweep.A0); - xf1.Position = _sweep.C0 - Common.Math.Mul(xf1.R, _sweep.LocalCenter); - - bool inRange = true; - for (Fixture f = _fixtureList; f != null; f = f.Next) - { - inRange = f.Synchronize(_world._broadPhase, xf1, _xf); - if (inRange == false) - { - break; - } - } - - if (inRange == false) - { - _flags |= BodyFlags.Frozen; - _linearVelocity.SetZero(); - _angularVelocity = 0.0f; - - // Failure - return false; - } - - // Success - return true; - } - - // This is used to prevent connected bodies from colliding. - // It may lie, depending on the collideConnected flag. - internal bool IsConnected(Body other) - { - for (JointEdge jn = _jointList; jn != null; jn = jn.Next) - { - if (jn.Other == other) - return jn.Joint._collideConnected == false; - } - - return false; - } - /// - /// Creates a fixture and attach it to this body. + /// Creates a shape and attach it to this body. /// @warning This function is locked during callbacks. /// - /// The fixture definition. - public Fixture CreateFixture(FixtureDef def) + /// The shape definition. + /// + public Shape CreateShape(ShapeDef shapeDef) { Box2DXDebug.Assert(_world._lock == false); if (_world._lock == true) @@ -333,28 +283,31 @@ namespace Box2DX.Dynamics return null; } - BroadPhase broadPhase = _world._broadPhase; + Shape s = Shape.Create(shapeDef); - Fixture fixture = new Fixture(); - fixture.Create(broadPhase, this, _xf, def); + s._next = _shapeList; + _shapeList = s; + ++_shapeCount; - fixture._next = _fixtureList; - _fixtureList = fixture; - ++_fixtureCount; + s._body = this; - fixture._body = this; + // Add the shape to the world's broad-phase. + s.CreateProxy(_world._broadPhase, _xf); - return fixture; + // Compute the sweep radius for CCD. + s.UpdateSweepRadius(_sweep.LocalCenter); + + return s; } /// - /// Destroy a fixture. This removes the fixture from the broad-phase and - /// therefore destroys any contacts associated with this fixture. All fixtures + /// Destroy a shape. This removes the shape from the broad-phase and + /// therefore destroys any contacts associated with this shape. All shapes /// attached to a body are implicitly destroyed when the body is destroyed. /// @warning This function is locked during callbacks. /// - /// The fixture to be removed. - public void DestroyFixture(Fixture fixture) + /// The shape to be removed. + public void DestroyShape(Shape shape) { Box2DXDebug.Assert(_world._lock == false); if (_world._lock == true) @@ -362,35 +315,33 @@ namespace Box2DX.Dynamics return; } - Box2DXDebug.Assert(fixture.Body == this); + Box2DXDebug.Assert(shape.GetBody() == this); + shape.DestroyProxy(_world._broadPhase); - // Remove the fixture from this body's singly linked list. - Box2DXDebug.Assert(_fixtureCount > 0); - Fixture node = _fixtureList; + Box2DXDebug.Assert(_shapeCount > 0); + Shape node = _shapeList; bool found = false; while (node != null) { - if (node == fixture) + if (node == shape) { - //*node = fixture->m_next; - _fixtureList = fixture.Next; + _shapeList = shape._next; found = true; break; } - node = node.Next; + node = node._next; } // You tried to remove a shape that is not attached to this body. Box2DXDebug.Assert(found); - BroadPhase broadPhase = _world._broadPhase; + shape._body = null; + shape._next = null; - fixture.Destroy(broadPhase); - fixture._body = null; - fixture._next = null; + --_shapeCount; - --_fixtureCount; + Shape.Destroy(shape); } // TODO_ERIN adjust linear velocity and torque to account for movement of center. @@ -398,8 +349,9 @@ namespace Box2DX.Dynamics /// Set the mass properties. Note that this changes the center of mass position. /// If you are not sure how to compute mass properties, use SetMassFromShapes. /// The inertia tensor is assumed to be relative to the center of mass. + /// @param massData the mass properties. /// - /// The mass properties. + /// public void SetMass(MassData massData) { Box2DXDebug.Assert(_world._lock == false); @@ -419,17 +371,26 @@ namespace Box2DX.Dynamics _invMass = 1.0f / _mass; } - _I = massData.I; + if ((_flags & BodyFlags.FixedRotation) == 0) + { + _I = massData.I; + } - if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0) + if (_I > 0.0f) { - _invI = 1.0f / _I; + _invI = 1.0f /_I; } // Move center of mass. _sweep.LocalCenter = massData.Center; _sweep.C0 = _sweep.C = Common.Math.Mul(_xf, _sweep.LocalCenter); + // Update the sweep radii of all child shapes. + for (Shape s = _shapeList; s != null; s = s._next) + { + s.UpdateSweepRadius(_sweep.LocalCenter); + } + BodyType oldType = _type; if (_invMass == 0.0f && _invI == 0.0f) { @@ -443,9 +404,9 @@ namespace Box2DX.Dynamics // If the body type changed, we need to refilter the broad-phase proxies. if (oldType != _type) { - for (Fixture f = _fixtureList; f != null; f = f.Next) + for (Shape s = _shapeList; s!=null; s = s._next) { - f.RefilterProxy(_world._broadPhase, _xf); + s.RefilterProxy(_world._broadPhase, _xf); } } } @@ -471,10 +432,10 @@ namespace Box2DX.Dynamics _invI = 0.0f; Vec2 center = Vec2.Zero; - for (Fixture f = _fixtureList; f != null; f = f.Next) + for (Shape s = _shapeList; s!=null; s = s._next) { MassData massData; - f.ComputeMass(out massData); + s.ComputeMass(out massData); _mass += massData.Mass; center += massData.Mass * massData.Center; _I += massData.I; @@ -504,6 +465,12 @@ namespace Box2DX.Dynamics _sweep.LocalCenter = center; _sweep.C0 = _sweep.C = Common.Math.Mul(_xf, _sweep.LocalCenter); + // Update the sweep radii of all child shapes. + for (Shape s = _shapeList; s != null; s = s._next) + { + s.UpdateSweepRadius(_sweep.LocalCenter); + } + BodyType oldType = _type; if (_invMass == 0.0f && _invI == 0.0f) { @@ -517,9 +484,9 @@ namespace Box2DX.Dynamics // If the body type changed, we need to refilter the broad-phase proxies. if (oldType != _type) { - for (Fixture f = _fixtureList; f != null; f = f.Next) + for (Shape s = _shapeList; s!=null; s = s._next) { - f.RefilterProxy(_world._broadPhase, _xf); + s.RefilterProxy(_world._broadPhase, _xf); } } } @@ -553,9 +520,9 @@ namespace Box2DX.Dynamics _sweep.A0 = _sweep.A = angle; bool freeze = false; - for (Fixture f = _fixtureList; f != null; f = f.Next) + for (Shape s = _shapeList; s != null; s = s._next) { - bool inRange = f.Synchronize(_world._broadPhase, _xf, _xf); + bool inRange = s.Synchronize(_world._broadPhase, _xf, _xf); if (inRange == false) { @@ -569,6 +536,10 @@ namespace Box2DX.Dynamics _flags |= BodyFlags.Frozen; _linearVelocity.SetZero(); _angularVelocity = 0.0f; + for (Shape s = _shapeList; s != null; s = s._next) + { + s.DestroyProxy(_world._broadPhase); + } // Failure return false; @@ -579,20 +550,6 @@ namespace Box2DX.Dynamics return true; } - /// - /// Set the position of the body's origin and rotation (radians). - /// This breaks any contacts and wakes the other bodies. - /// Note this is less efficient than the other overload - you should use that - /// if the angle is available. - /// - /// The transform of position and angle to set the body to. - /// False if the movement put a shape outside the world. In this case the - /// body is automatically frozen. - public bool SetXForm(XForm xf) - { - return SetXForm(xf.Position, xf.GetAngle()); - } - /// /// Get the body transform for the body's origin. /// @@ -602,24 +559,6 @@ namespace Box2DX.Dynamics return _xf; } - /// - /// Set the world body origin position. - /// - /// The new position of the body. - public void SetPosition(Vec2 position) - { - SetXForm(position, GetAngle()); - } - - /// - /// Set the world body angle. - /// - /// The new angle of the body. - public void SetAngle(float angle) - { - SetXForm(GetPosition(), angle); - } - /// /// Get the world body origin position. /// @@ -678,9 +617,9 @@ namespace Box2DX.Dynamics /// Set the angular velocity. /// /// The new angular velocity in radians/second. - public void SetAngularVelocity(float w) + public void SetAngularVelocity(float omega) { - _angularVelocity = w; + _angularVelocity = omega; } /// @@ -714,7 +653,7 @@ namespace Box2DX.Dynamics /// without affecting the linear velocity of the center of mass. /// This wakes up the body. /// - /// Torque about the z-axis (out of the screen), usually in N-m. + /// About the z-axis (out of the screen), usually in N-m. public void ApplyTorque(float torque) { if (IsSleeping()) @@ -759,19 +698,6 @@ namespace Box2DX.Dynamics return _I; } - /// - /// Get the mass data of the body. - /// - /// A struct containing the mass, inertia and center of the body. - public MassData GetMassData() - { - MassData massData = new MassData(); - massData.Mass = _mass; - massData.I = _I; - massData.Center = GetWorldCenter(); - return massData; - } - /// /// Get the world coordinates of a point given the local coordinates. /// @@ -832,26 +758,6 @@ namespace Box2DX.Dynamics return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint)); } - public float GetLinearDamping() - { - return _linearDamping; - } - - public void SetLinearDamping(float linearDamping) - { - _linearDamping = linearDamping; - } - - public float GetAngularDamping() - { - return _angularDamping; - } - - public void SetAngularDamping(float angularDamping) - { - _angularDamping = angularDamping; - } - /// /// Is this body treated like a bullet for continuous collision detection? /// @@ -877,31 +783,6 @@ namespace Box2DX.Dynamics } } - public bool IsFixedRotation() - { - return (_flags & BodyFlags.FixedRotation) == BodyFlags.FixedRotation; - } - - public void SetFixedRotation(bool fixedr) - { - if (fixedr) - { - _angularVelocity = 0.0f; - _invI = 0.0f; - _flags |= BodyFlags.FixedRotation; - } - else - { - if (_I > 0.0f) - { - // Recover _invI from _I. - _invI = 1.0f / _I; - _flags &= BodyFlags.FixedRotation; - } - // TODO: Else what? - } - } - /// /// Is this body static (immovable)? /// @@ -911,22 +792,6 @@ namespace Box2DX.Dynamics return _type == BodyType.Static; } - public void SetStatic() - { - if (_type == BodyType.Static) - return; - _mass = 0.0f; - _invMass = 0.0f; - _I = 0.0f; - _invI = 0.0f; - _type = BodyType.Static; - - for (Fixture f = _fixtureList; f != null; f = f.Next) - { - f.RefilterProxy(_world._broadPhase, _xf); - } - } - /// /// Is this body dynamic (movable)? /// @@ -954,11 +819,6 @@ namespace Box2DX.Dynamics return (_flags & BodyFlags.Sleep) == BodyFlags.Sleep; } - public bool IsAllowSleeping() - { - return (_flags & BodyFlags.AllowSleep) == BodyFlags.AllowSleep; - } - /// /// You can disable sleeping on this body. /// @@ -1000,12 +860,12 @@ namespace Box2DX.Dynamics } /// - /// Get the list of all fixtures attached to this body. + /// Get the list of all shapes attached to this body. /// /// - public Fixture GetFixtureList() + public Shape GetShapeList() { - return _fixtureList; + return _shapeList; } /// @@ -1017,10 +877,6 @@ namespace Box2DX.Dynamics return _jointList; } - public Controllers.ControllerEdge GetControllerList() - { - return _controllerList; - } /// /// Get the next body in the world's body list. @@ -1052,12 +908,59 @@ namespace Box2DX.Dynamics /// public World GetWorld() { return _world; } + internal bool SynchronizeShapes() + { + XForm xf1 = new XForm(); + xf1.R.Set(_sweep.A0); + xf1.Position = _sweep.C0 - Common.Math.Mul(xf1.R, _sweep.LocalCenter); + + bool inRange = true; + for (Shape s = _shapeList; s != null; s = s._next) + { + inRange = s.Synchronize(_world._broadPhase, xf1, _xf); + if (inRange == false) + { + break; + } + } + + if (inRange == false) + { + _flags |= BodyFlags.Frozen; + _linearVelocity.SetZero(); + _angularVelocity = 0.0f; + for (Shape s = _shapeList; s != null; s = s._next) + { + s.DestroyProxy(_world._broadPhase); + } + + // Failure + return false; + } + + // Success + return true; + } + internal void SynchronizeTransform() { _xf.R.Set(_sweep.A); _xf.Position = _sweep.C - Common.Math.Mul(_xf.R, _sweep.LocalCenter); } + // This is used to prevent connected bodies from colliding. + // It may lie, depending on the collideConnected flag. + internal bool IsConnected(Body other) + { + for (JointEdge jn = _jointList; jn != null; jn = jn.Next) + { + if (jn.Other == other) + return jn.Joint._collideConnected == false; + } + + return false; + } + internal void Advance(float t) { // Advance to the new safe time. diff --git a/Box2d/Assets/Program/Box2d/Dynamics/ContactManager.cs b/Box2d/Assets/Program/Box2d/Dynamics/ContactManager.cs index 667c848..2b91ed1 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/ContactManager.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/ContactManager.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -19,6 +19,11 @@ 3. This notice may not be removed or altered from any source distribution. */ +using System; +using System.Collections.Generic; +using System.Text; + +using Box2DX.Common; using Box2DX.Collision; namespace Box2DX.Dynamics @@ -36,40 +41,44 @@ namespace Box2DX.Dynamics public bool _destroyImmediate; - public ContactManager() { } + public ContactManager() + { + _world = null; + _destroyImmediate = false; + } // This is a callback from the broadphase when two AABB proxies begin // to overlap. We create a Contact to manage the narrow phase. - public override object PairAdded(object proxyUserDataA, object proxyUserDataB) + public override object PairAdded(object proxyUserData1, object proxyUserData2) { - Fixture fixtureA = proxyUserDataA as Fixture; - Fixture fixtureB = proxyUserDataB as Fixture; + Shape shape1 = proxyUserData1 as Shape; + Shape shape2 = proxyUserData2 as Shape; - Body bodyA = fixtureA.Body; - Body bodyB = fixtureB.Body; + Body body1 = shape1.GetBody(); + Body body2 = shape2.GetBody(); - if (bodyA.IsStatic() && bodyB.IsStatic()) + if (body1.IsStatic() && body2.IsStatic()) { return _nullContact; } - if (fixtureA.Body == fixtureB.Body) + if (shape1.GetBody() == shape2.GetBody()) { return _nullContact; } - if (bodyB.IsConnected(bodyA)) + if (body2.IsConnected(body1)) { return _nullContact; } - if (_world._contactFilter != null && _world._contactFilter.ShouldCollide(fixtureA, fixtureB) == false) + if (_world._contactFilter != null && _world._contactFilter.ShouldCollide(shape1, shape2) == false) { return _nullContact; } // Call the factory. - Contact c = Contact.Create(fixtureA, fixtureB); + Contact c = Contact.Create(shape1, shape2); if (c == null) { @@ -77,10 +86,10 @@ namespace Box2DX.Dynamics } // Contact creation may swap shapes. - fixtureA = c.FixtureA; - fixtureB = c.FixtureB; - bodyA = fixtureA.Body; - bodyB = fixtureB.Body; + shape1 = c.GetShape1(); + shape2 = c.GetShape2(); + body1 = shape1.GetBody(); + body2 = shape2.GetBody(); // Insert into the world. c._prev = null; @@ -94,28 +103,28 @@ namespace Box2DX.Dynamics // Connect to island graph. // Connect to body 1 - c._nodeA.Contact = c; - c._nodeA.Other = bodyB; + c._node1.Contact = c; + c._node1.Other = body2; - c._nodeA.Prev = null; - c._nodeA.Next = bodyA._contactList; - if (bodyA._contactList != null) + c._node1.Prev = null; + c._node1.Next = body1._contactList; + if (body1._contactList != null) { - bodyA._contactList.Prev = c._nodeA; + body1._contactList.Prev = c._node1; } - bodyA._contactList = c._nodeA; + body1._contactList = c._node1; // Connect to body 2 - c._nodeB.Contact = c; - c._nodeB.Other = bodyA; + c._node2.Contact = c; + c._node2.Other = body1; - c._nodeB.Prev = null; - c._nodeB.Next = bodyB._contactList; - if (bodyB._contactList != null) + c._node2.Prev = null; + c._node2.Next = body2._contactList; + if (body2._contactList != null) { - bodyB._contactList.Prev = c._nodeB; + body2._contactList.Prev = c._node2; } - bodyB._contactList = c._nodeB; + body2._contactList = c._node2; ++_world._contactCount; return c; @@ -146,15 +155,40 @@ namespace Box2DX.Dynamics public void Destroy(Contact c) { - Fixture fixtureA = c.FixtureA; - Fixture fixtureB = c.FixtureB; - Body bodyA = fixtureA.Body; - Body bodyB = fixtureB.Body; - - if (c.Manifold.PointCount > 0) + Shape shape1 = c.GetShape1(); + Shape shape2 = c.GetShape2(); + Body body1 = shape1.GetBody(); + Body body2 = shape2.GetBody(); + + ContactPoint cp = new ContactPoint(); + cp.Shape1 = shape1; + cp.Shape2 = shape2; + cp.Friction = Settings.MixFriction(shape1.Friction, shape2.Friction); + cp.Restitution = Settings.MixRestitution(shape1.Restitution, shape2.Restitution); + + // Inform the user that this contact is ending. + int manifoldCount = c.GetManifoldCount(); + if (manifoldCount > 0 && _world._contactListener!=null) { - if(_world._contactListener!=null) - _world._contactListener.EndContact(c); + Manifold[] manifolds = c.GetManifolds(); + + for (int i = 0; i < manifoldCount; ++i) + { + Manifold manifold = manifolds[i]; + cp.Normal = manifold.Normal; + + for (int j = 0; j < manifold.PointCount; ++j) + { + ManifoldPoint mp = manifold.Points[j]; + cp.Position = body1.GetWorldPoint(mp.LocalPoint1); + Vec2 v1 = body1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1); + Vec2 v2 = body2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2); + cp.Velocity = v2 - v1; + cp.Separation = mp.Separation; + cp.ID = mp.ID; + _world._contactListener.Remove(cp); + } + } } // Remove from the world. @@ -174,39 +208,39 @@ namespace Box2DX.Dynamics } // Remove from body 1 - if (c._nodeA.Prev != null) + if (c._node1.Prev != null) { - c._nodeA.Prev.Next = c._nodeA.Next; + c._node1.Prev.Next = c._node1.Next; } - if (c._nodeA.Next != null) + if (c._node1.Next != null) { - c._nodeA.Next.Prev = c._nodeA.Prev; + c._node1.Next.Prev = c._node1.Prev; } - if (c._nodeA == bodyA._contactList) + if (c._node1 == body1._contactList) { - bodyA._contactList = c._nodeA.Next; + body1._contactList = c._node1.Next; } // Remove from body 2 - if (c._nodeB.Prev != null) + if (c._node2.Prev != null) { - c._nodeB.Prev.Next = c._nodeB.Next; + c._node2.Prev.Next = c._node2.Next; } - if (c._nodeB.Next != null) + if (c._node2.Next != null) { - c._nodeB.Next.Prev = c._nodeB.Prev; + c._node2.Next.Prev = c._node2.Prev; } - if (c._nodeB == bodyB._contactList) + if (c._node2 == body2._contactList) { - bodyB._contactList = c._nodeB.Next; + body2._contactList = c._node2.Next; } // Call the factory. - Contact.Destroy(ref c); + Contact.Destroy(c); --_world._contactCount; } @@ -218,9 +252,9 @@ namespace Box2DX.Dynamics // Update awake contacts. for (Contact c = _world._contactList; c != null; c = c.GetNext()) { - Body bodyA = c._fixtureA.Body; - Body bodyB = c._fixtureB.Body; - if (bodyA.IsSleeping() && bodyB.IsSleeping()) + Body body1 = c.GetShape1().GetBody(); + Body body2 = c.GetShape2().GetBody(); + if (body1.IsSleeping() && body2.IsSleeping()) { continue; } diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/CircleContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/CircleContact.cs index 98fe5fd..2d86681 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/CircleContact.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/CircleContact.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -19,6 +19,10 @@ 3. This notice may not be removed or altered from any source distribution. */ +using System; +using System.Collections.Generic; +using System.Text; + using Box2DX.Collision; using Box2DX.Common; @@ -26,25 +30,105 @@ namespace Box2DX.Dynamics { public class CircleContact : Contact { - public CircleContact(Fixture fixtureA, Fixture fixtureB) - : base(fixtureA, fixtureB) + public Manifold _manifold = new Manifold(); + + public override Manifold[] GetManifolds() + { + return new Manifold[] { _manifold }; + } + + public CircleContact(Shape s1, Shape s2) + : base(s1, s2) { - Box2DXDebug.Assert(fixtureA.ShapeType == ShapeType.CircleShape); - Box2DXDebug.Assert(fixtureB.ShapeType == ShapeType.CircleShape); - CollideShapeFunction = CollideCircles; + Box2DXDebug.Assert(_shape1.GetType() == ShapeType.CircleShape); + Box2DXDebug.Assert(_shape2.GetType() == ShapeType.CircleShape); + _manifold.PointCount = 0; + _manifold.Points[0].NormalImpulse = 0.0f; + _manifold.Points[0].TangentImpulse = 0.0f; } - private static void CollideCircles(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) + public override void Evaluate(ContactListener listener) { - Collision.Collision.CollideCircles(ref manifold, (CircleShape)shape1, xf1, (CircleShape)shape2, xf2); + Body b1 = _shape1.GetBody(); + Body b2 = _shape2.GetBody(); +#warning "needfix" + //memcpy(&m0, &m_manifold, sizeof(b2Manifold)); + Manifold m0 = _manifold.Clone(); + + Collision.Collision.CollideCircles(ref _manifold, (CircleShape)_shape1, b1.GetXForm(), + (CircleShape)_shape2, b2.GetXForm()); + + ContactPoint cp = new ContactPoint(); + cp.Shape1 = _shape1; + cp.Shape2 = _shape2; + cp.Friction = Settings.MixFriction(_shape1.Friction, _shape2.Friction); + cp.Restitution = Settings.MixRestitution(_shape1.Restitution, _shape2.Restitution); + + if (_manifold.PointCount > 0) + { + _manifoldCount = 1; + ManifoldPoint mp = _manifold.Points[0]; + + if (m0.PointCount == 0) + { + mp.NormalImpulse = 0.0f; + mp.TangentImpulse = 0.0f; + + if (listener!=null) + { + cp.Position = b1.GetWorldPoint(mp.LocalPoint1); + Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1); + Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2); + cp.Velocity = v2 - v1; + cp.Normal = _manifold.Normal; + cp.Separation = mp.Separation; + cp.ID = mp.ID; + listener.Add(cp); + } + } + else + { + ManifoldPoint mp0 = m0.Points[0]; + mp.NormalImpulse = mp0.NormalImpulse; + mp.TangentImpulse = mp0.TangentImpulse; + + if (listener!=null) + { + cp.Position = b1.GetWorldPoint(mp.LocalPoint1); + Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1); + Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2); + cp.Velocity = v2 - v1; + cp.Normal = _manifold.Normal; + cp.Separation = mp.Separation; + cp.ID = mp.ID; + listener.Persist(cp); + } + } + } + else + { + _manifoldCount = 0; + if (m0.PointCount > 0 && listener!=null) + { + ManifoldPoint mp0 = m0.Points[0]; + cp.Position = b1.GetWorldPoint(mp0.LocalPoint1); + Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp0.LocalPoint1); + Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp0.LocalPoint2); + cp.Velocity = v2 - v1; + cp.Normal = m0.Normal; + cp.Separation = mp0.Separation; + cp.ID = mp0.ID; + listener.Remove(cp); + } + } } - new public static Contact Create(Fixture fixtureA, Fixture fixtureB) + new public static Contact Create(Shape shape1, Shape shape2) { - return new CircleContact(fixtureA, fixtureB); + return new CircleContact(shape1, shape2); } - new public static void Destroy(ref Contact contact) + new public static void Destroy(Contact contact) { contact = null; } diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/Contact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/Contact.cs index 7c23ca5..127e610 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/Contact.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/Contact.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -20,13 +20,16 @@ */ using System; +using System.Collections.Generic; +using System.Text; + using Box2DX.Collision; using Box2DX.Common; namespace Box2DX.Dynamics { - public delegate Contact ContactCreateFcn(Fixture fixtureA, Fixture fixtureB); - public delegate void ContactDestroyFcn(ref Contact contact); + public delegate Contact ContactCreateFcn(Shape shape1, Shape shape2); + public delegate void ContactDestroyFcn(Contact contact); public struct ContactRegister { @@ -35,6 +38,7 @@ namespace Box2DX.Dynamics public bool Primary; } +#warning "CAS" /// /// A contact edge is used to connect bodies and contacts together /// in a contact graph where each body is a node and each contact @@ -62,6 +66,86 @@ namespace Box2DX.Dynamics public ContactEdge Next; } +#warning "CAS" + /// + /// This structure is used to report contact points. + /// + public class ContactPoint + { + /// + /// The first shape. + /// + public Shape Shape1; + /// + /// The second shape. + /// + public Shape Shape2; + /// + /// Position in world coordinates. + /// + public Vec2 Position; + /// + /// Velocity of point on body2 relative to point on body1 (pre-solver). + /// + public Vec2 Velocity; + /// + /// Points from shape1 to shape2. + /// + public Vec2 Normal; + /// + /// The separation is negative when shapes are touching. + /// + public float Separation; + /// + /// The combined friction coefficient. + /// + public float Friction; + /// + /// The combined restitution coefficient. + /// + public float Restitution; + /// + /// The contact id identifies the features in contact. + /// + public ContactID ID; + } + +#warning "CAS" + /// + /// This structure is used to report contact point results. + /// + public class ContactResult + { + /// + /// The first shape. + /// + public Shape Shape1; + /// + /// The second shape. + /// + public Shape Shape2; + /// + /// Position in world coordinates. + /// + public Vec2 Position; + /// + /// Points from shape1 to shape2. + /// + public Vec2 Normal; + /// + /// The normal impulse applied to body2. + /// + public float NormalImpulse; + /// + /// The tangent impulse applied to body2. + /// + public float TangentImpulse; + /// + /// The contact id identifies the features in contact. + /// + public ContactID ID; + } + /// /// The class manages contact between two shapes. A contact exists for each overlapping /// AABB in the broad-phase (except if filtered). Therefore a contact object may exist @@ -75,56 +159,62 @@ namespace Box2DX.Dynamics NonSolid = 0x0001, Slow = 0x0002, Island = 0x0004, - Toi = 0x0008, - Touch = 0x0010 + Toi = 0x0008 } public static ContactRegister[][] s_registers = new ContactRegister[(int)ShapeType.ShapeTypeCount][/*(int)ShapeType.ShapeTypeCount*/]; - public static bool s_initialized; + public static bool s_initialized = false; public CollisionFlags _flags; + public int _manifoldCount; // World pool and list pointers. public Contact _prev; public Contact _next; // Nodes for connecting bodies. - public ContactEdge _nodeA; - public ContactEdge _nodeB; + public ContactEdge _node1; + public ContactEdge _node2; - public Fixture _fixtureA; - public Fixture _fixtureB; - - public Manifold _manifold = new Manifold(); + public Shape _shape1; + public Shape _shape2; public float _toi; - internal delegate void CollideShapeDelegate( - ref Manifold manifold, Shape circle1, XForm xf1, Shape circle2, XForm xf2); - internal CollideShapeDelegate CollideShapeFunction; - - public Contact(){} + public Contact() + { + _shape1 = null; _shape2 = null; + } - public Contact(Fixture fA, Fixture fB) + public Contact(Shape s1, Shape s2) { _flags = 0; - if (fA.IsSensor || fB.IsSensor) + if (s1.IsSensor || s2.IsSensor) { _flags |= CollisionFlags.NonSolid; } - _fixtureA = fA; - _fixtureB = fB; + _shape1 = s1; + _shape2 = s2; - _manifold.PointCount = 0; + _manifoldCount = 0; _prev = null; _next = null; - _nodeA = new ContactEdge(); - _nodeB = new ContactEdge(); + _node1 = new ContactEdge(); + _node1.Contact = null; + _node1.Prev = null; + _node1.Next = null; + _node1.Other = null; + + _node2 = new ContactEdge(); + _node2.Contact = null; + _node2.Prev = null; + _node2.Next = null; + _node2.Other = null; } public static void AddType(ContactCreateFcn createFcn, ContactDestroyFcn destoryFcn, @@ -142,6 +232,9 @@ namespace Box2DX.Dynamics if (type1 != type2) { + //if (_registers[(int)type2] == null) + // _registers[(int)type2] = new ContactRegister[(int)ShapeType.ShapeTypeCount]; + s_registers[(int)type2][(int)type1].CreateFcn = createFcn; s_registers[(int)type2][(int)type1].DestroyFcn = destoryFcn; s_registers[(int)type2][(int)type1].Primary = false; @@ -153,12 +246,9 @@ namespace Box2DX.Dynamics AddType(CircleContact.Create, CircleContact.Destroy, ShapeType.CircleShape, ShapeType.CircleShape); AddType(PolyAndCircleContact.Create, PolyAndCircleContact.Destroy, ShapeType.PolygonShape, ShapeType.CircleShape); AddType(PolygonContact.Create, PolygonContact.Destroy, ShapeType.PolygonShape, ShapeType.PolygonShape); - - AddType(EdgeAndCircleContact.Create, EdgeAndCircleContact.Destroy, ShapeType.EdgeShape, ShapeType.CircleShape); - AddType(PolyAndEdgeContact.Create, PolyAndEdgeContact.Destroy, ShapeType.PolygonShape, ShapeType.EdgeShape); } - public static Contact Create(Fixture fixtureA, Fixture fixtureB) + public static Contact Create(Shape shape1, Shape shape2) { if (s_initialized == false) { @@ -166,8 +256,8 @@ namespace Box2DX.Dynamics s_initialized = true; } - ShapeType type1 = fixtureA.ShapeType; - ShapeType type2 = fixtureB.ShapeType; + ShapeType type1 = shape1.GetType(); + ShapeType type2 = shape2.GetType(); Box2DXDebug.Assert(ShapeType.UnknownShape < type1 && type1 < ShapeType.ShapeTypeCount); Box2DXDebug.Assert(ShapeType.UnknownShape < type2 && type2 < ShapeType.ShapeTypeCount); @@ -177,11 +267,17 @@ namespace Box2DX.Dynamics { if (s_registers[(int)type1][(int)type2].Primary) { - return createFcn(fixtureA, fixtureB); + return createFcn(shape1, shape2); } else { - return createFcn(fixtureB, fixtureA); + Contact c = createFcn(shape2, shape1); + for (int i = 0; i < c.GetManifoldCount(); ++i) + { + Manifold m = c.GetManifolds()[i]; + m.Normal = -m.Normal; + } + return c; } } else @@ -190,187 +286,107 @@ namespace Box2DX.Dynamics } } - public static void Destroy(ref Contact contact) + public static void Destroy(Contact contact) { Box2DXDebug.Assert(s_initialized == true); - if (contact._manifold.PointCount > 0) + if (contact.GetManifoldCount() > 0) { - contact.FixtureA.Body.WakeUp(); - contact.FixtureB.Body.WakeUp(); + contact.GetShape1().GetBody().WakeUp(); + contact.GetShape2().GetBody().WakeUp(); } - ShapeType typeA = contact.FixtureA.ShapeType; - ShapeType typeB = contact.FixtureB.ShapeType; - - Box2DXDebug.Assert(ShapeType.UnknownShape < typeA && typeA < ShapeType.ShapeTypeCount); - Box2DXDebug.Assert(ShapeType.UnknownShape < typeB && typeB < ShapeType.ShapeTypeCount); + ShapeType type1 = contact.GetShape1().GetType(); + ShapeType type2 = contact.GetShape2().GetType(); - ContactDestroyFcn destroyFcn = s_registers[(int)typeA][(int)typeB].DestroyFcn; - destroyFcn(ref contact); - } - - public void Update(ContactListener listener) - { - Manifold oldManifold = _manifold.Clone(); - - Evaluate(); - - Body bodyA = _fixtureA.Body; - Body bodyB = _fixtureB.Body; - - int oldCount = oldManifold.PointCount; - int newCount = _manifold.PointCount; - - if (newCount == 0 && oldCount > 0) - { - bodyA.WakeUp(); - bodyB.WakeUp(); - } - - // Slow contacts don't generate TOI events. - if (bodyA.IsStatic() || bodyA.IsBullet() || bodyB.IsStatic() || bodyB.IsBullet()) - { - _flags &= ~CollisionFlags.Slow; - } - else - { - _flags |= CollisionFlags.Slow; - } - - // Match old contact ids to new contact ids and copy the - // stored impulses to warm start the solver. - for (int i = 0; i < _manifold.PointCount; ++i) - { - ManifoldPoint mp2 = _manifold.Points[i]; - mp2.NormalImpulse = 0.0f; - mp2.TangentImpulse = 0.0f; - ContactID id2 = mp2.ID; - - for (int j = 0; j < oldManifold.PointCount; ++j) - { - ManifoldPoint mp1 = oldManifold.Points[j]; - - if (mp1.ID.Key == id2.Key) - { - mp2.NormalImpulse = mp1.NormalImpulse; - mp2.TangentImpulse = mp1.TangentImpulse; - break; - } - } - } - - if (oldCount == 0 && newCount > 0) - { - _flags |= CollisionFlags.Touch; - if(listener!=null) - listener.BeginContact(this); - } - - if (oldCount > 0 && newCount == 0) - { - _flags &= ~CollisionFlags.Touch; - if (listener != null) - listener.EndContact(this); - } - - if ((_flags & CollisionFlags.NonSolid) == 0) - { - if (listener != null) - listener.PreSolve(this, oldManifold); - - // The user may have disabled contact. - if (_manifold.PointCount == 0) - { - _flags &= ~CollisionFlags.Touch; - } - } - } - - public void Evaluate() - { - Body bodyA = _fixtureA.Body; - Body bodyB = _fixtureB.Body; - - Box2DXDebug.Assert(CollideShapeFunction!=null); - - CollideShapeFunction(ref _manifold, _fixtureA.Shape, bodyA.GetXForm(), _fixtureB.Shape, bodyB.GetXForm()); - } + Box2DXDebug.Assert(ShapeType.UnknownShape < type1 && type1 < ShapeType.ShapeTypeCount); + Box2DXDebug.Assert(ShapeType.UnknownShape < type2 && type2 < ShapeType.ShapeTypeCount); - public float ComputeTOI(Sweep sweepA, Sweep sweepB) - { - TOIInput input = new TOIInput(); - input.SweepA = sweepA; - input.SweepB = sweepB; - input.SweepRadiusA = _fixtureA.ComputeSweepRadius(sweepA.LocalCenter); - input.SweepRadiusB = _fixtureB.ComputeSweepRadius(sweepB.LocalCenter); - input.Tolerance = Common.Settings.LinearSlop; - - return Collision.Collision.TimeOfImpact(input, _fixtureA.Shape, _fixtureB.Shape); + ContactDestroyFcn destroyFcn = s_registers[(int)type1][(int)type2].DestroyFcn; + destroyFcn(contact); } /// - /// Get the contact manifold. + /// Get the manifold array. /// - public Manifold Manifold - { - get { return _manifold; } - } + /// + public abstract Manifold[] GetManifolds(); /// - /// Get the world manifold. - /// - public void GetWorldManifold(out WorldManifold worldManifold) + /// Get the number of manifolds. This is 0 or 1 between convex shapes. + /// This may be greater than 1 for convex-vs-concave shapes. Each + /// manifold holds up to two contact points with a shared contact normal. + /// + /// + public int GetManifoldCount() { - worldManifold = new WorldManifold(); - - Body bodyA = _fixtureA.Body; - Body bodyB = _fixtureB.Body; - Shape shapeA = _fixtureA.Shape; - Shape shapeB = _fixtureB.Shape; - - worldManifold.Initialize(_manifold, bodyA.GetXForm(), shapeA._radius, bodyB.GetXForm(), shapeB._radius); + return _manifoldCount; } /// /// Is this contact solid? /// /// True if this contact should generate a response. - public bool IsSolid + public bool IsSolid() { - get { return (_flags & CollisionFlags.NonSolid) == 0; } - } - - /// - /// Are fixtures touching? - /// - public bool AreTouching - { - get { return (_flags & CollisionFlags.Touch) == CollisionFlags.Touch; } + return (_flags & CollisionFlags.NonSolid) == 0; } /// /// Get the next contact in the world's contact list. /// + /// public Contact GetNext() { return _next; } /// - /// Get the first fixture in this contact. + /// Get the first shape in this contact. /// - public Fixture FixtureA + /// + public Shape GetShape1() { - get { return _fixtureA; } + return _shape1; } /// - /// Get the second fixture in this contact. + /// Get the second shape in this contact. /// - public Fixture FixtureB + /// + public Shape GetShape2() + { + return _shape2; + } + + public void Update(ContactListener listener) { - get { return _fixtureB; } - } + int oldCount = GetManifoldCount(); + + Evaluate(listener); + + int newCount = GetManifoldCount(); + + Body body1 = _shape1.GetBody(); + Body body2 = _shape2.GetBody(); + + if (newCount == 0 && oldCount > 0) + { + body1.WakeUp(); + body2.WakeUp(); + } + + // Slow contacts don't generate TOI events. + if (body1.IsStatic() || body1.IsBullet() || body2.IsStatic() || body2.IsBullet()) + { + _flags &= ~CollisionFlags.Slow; + } + else + { + _flags |= CollisionFlags.Slow; + } + } + + public abstract void Evaluate(ContactListener listener); } } diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/ContactSolver.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/ContactSolver.cs index a8affb6..2578848 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/ContactSolver.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/ContactSolver.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -20,48 +20,52 @@ */ //#define B2_DEBUG_SOLVER +#define FIRST using System; +using System.Collections.Generic; +using System.Text; + using Box2DX.Collision; using Box2DX.Common; namespace Box2DX.Dynamics { - public struct ContactConstraintPoint +#warning "CAS" + public class ContactConstraintPoint { - public Vec2 LocalPoint; - public Vec2 RA; - public Vec2 RB; + public Vec2 LocalAnchor1; + public Vec2 LocalAnchor2; + public Vec2 R1; + public Vec2 R2; public float NormalImpulse; public float TangentImpulse; public float NormalMass; public float TangentMass; public float EqualizedMass; + public float Separation; public float VelocityBias; } +#warning "CAS" public class ContactConstraint { public ContactConstraintPoint[] Points = new ContactConstraintPoint[Settings.MaxManifoldPoints]; - public Vec2 LocalPlaneNormal; - public Vec2 LocalPoint; public Vec2 Normal; public Mat22 NormalMass; public Mat22 K; - public Body BodyA; - public Body BodyB; - public ManifoldType Type; - public float Radius; + public Manifold Manifold; + public Body Body1; + public Body Body2; public float Friction; public float Restitution; public int PointCount; - public Manifold Manifold; - //public ContactConstraint() - //{ - // for (int i = 0; i < Settings.MaxManifoldPoints; i++) - // Points[i] = new ContactConstraintPoint(); - //} + public ContactConstraint() + { + for (int i = 0; i < Settings.MaxManifoldPoints; i++) + Points[i] = new ContactConstraintPoint(); + } } public class ContactSolver : IDisposable @@ -73,147 +77,156 @@ namespace Box2DX.Dynamics public ContactSolver(TimeStep step, Contact[] contacts, int contactCount) { _step = step; - _constraintCount = contactCount; + + _constraintCount = 0; + for (int i = 0; i < contactCount; ++i) + { + Box2DXDebug.Assert(contacts[i].IsSolid()); + _constraintCount += contacts[i].GetManifoldCount(); + } _constraints = new ContactConstraint[_constraintCount]; for (int i = 0; i < _constraintCount; i++) _constraints[i] = new ContactConstraint(); int count = 0; - for (int i = 0; i < _constraintCount; ++i) + for (int i = 0; i < contactCount; ++i) { Contact contact = contacts[i]; - Fixture fixtureA = contact._fixtureA; - Fixture fixtureB = contact._fixtureB; - Shape shapeA = fixtureA.Shape; - Shape shapeB = fixtureB.Shape; - float radiusA = shapeA._radius; - float radiusB = shapeB._radius; - Body bodyA = fixtureA.Body; - Body bodyB = fixtureB.Body; - Manifold manifold = contact.Manifold; - - float friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction); - float restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution); - - Vec2 vA = bodyA._linearVelocity; - Vec2 vB = bodyB._linearVelocity; - float wA = bodyA._angularVelocity; - float wB = bodyB._angularVelocity; - - Box2DXDebug.Assert(manifold.PointCount > 0); - - WorldManifold worldManifold = new WorldManifold(); - worldManifold.Initialize(manifold, bodyA._xf, radiusA, bodyB._xf, radiusB); - - ContactConstraint cc = _constraints[i]; - cc.BodyA = bodyA; - cc.BodyB = bodyB; - cc.Manifold = manifold; - cc.Normal = worldManifold.Normal; - cc.PointCount = manifold.PointCount; - cc.Friction = friction; - cc.Restitution = restitution; - - cc.LocalPlaneNormal = manifold.LocalPlaneNormal; - cc.LocalPoint = manifold.LocalPoint; - cc.Radius = radiusA + radiusB; - cc.Type = manifold.Type; - - unsafe + Shape shape1 = contact._shape1; + Shape shape2 = contact._shape2; + Body b1 = shape1.GetBody(); + Body b2 = shape2.GetBody(); + int manifoldCount = contact.GetManifoldCount(); + Manifold[] manifolds = contact.GetManifolds(); + + float friction = Settings.MixFriction(shape1.Friction, shape2.Friction); + float restitution = Settings.MixRestitution(shape1.Restitution, shape2.Restitution); + + Vec2 v1 = b1._linearVelocity; + Vec2 v2 = b2._linearVelocity; + float w1 = b1._angularVelocity; + float w2 = b2._angularVelocity; + + for (int j = 0; j < manifoldCount; ++j) { - fixed (ContactConstraintPoint* ccPointsPtr = cc.Points) - { - for (int j = 0; j < cc.PointCount; ++j) - { - ManifoldPoint cp = manifold.Points[j]; - ContactConstraintPoint* ccp = &ccPointsPtr[j]; + Manifold manifold = manifolds[j]; - ccp->NormalImpulse = cp.NormalImpulse; - ccp->TangentImpulse = cp.TangentImpulse; + Box2DXDebug.Assert(manifold.PointCount > 0); + + Vec2 normal = manifold.Normal; + + Box2DXDebug.Assert(count < _constraintCount); + ContactConstraint cc = _constraints[count]; + cc.Body1 = b1; + cc.Body2 = b2; + cc.Manifold = manifold; + cc.Normal = normal; + cc.PointCount = manifold.PointCount; + cc.Friction = friction; + cc.Restitution = restitution; + + for (int k = 0; k < cc.PointCount; ++k) + { + ManifoldPoint cp = manifold.Points[k]; + ContactConstraintPoint ccp = cc.Points[k]; - ccp->LocalPoint = cp.LocalPoint; + ccp.NormalImpulse = cp.NormalImpulse; + ccp.TangentImpulse = cp.TangentImpulse; + ccp.Separation = cp.Separation; - ccp->RA = worldManifold.Points[j] - bodyA._sweep.C; - ccp->RB = worldManifold.Points[j] - bodyB._sweep.C; + ccp.LocalAnchor1 = cp.LocalPoint1; + ccp.LocalAnchor2 = cp.LocalPoint2; + ccp.R1 = Common.Math.Mul(b1.GetXForm().R, cp.LocalPoint1 - b1.GetLocalCenter()); + ccp.R2 = Common.Math.Mul(b2.GetXForm().R, cp.LocalPoint2 - b2.GetLocalCenter()); - float rnA = Vec2.Cross(ccp->RA, cc.Normal); - float rnB = Vec2.Cross(ccp->RB, cc.Normal); - rnA *= rnA; - rnB *= rnB; + float rn1 = Vec2.Cross(ccp.R1, normal); + float rn2 = Vec2.Cross(ccp.R2, normal); + rn1 *= rn1; + rn2 *= rn2; - float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB; + float kNormal = b1._invMass + b2._invMass + b1._invI * rn1 + b2._invI * rn2; - Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON); - ccp->NormalMass = 1.0f / kNormal; + Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON); + ccp.NormalMass = 1.0f / kNormal; - float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass; - kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB; + float kEqualized = b1._mass * b1._invMass + b2._mass * b2._invMass; + kEqualized += b1._mass * b1._invI * rn1 + b2._mass * b2._invI * rn2; - Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON); - ccp->EqualizedMass = 1.0f / kEqualized; + Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON); + ccp.EqualizedMass = 1.0f / kEqualized; - Vec2 tangent = Vec2.Cross(cc.Normal, 1.0f); + Vec2 tangent = Vec2.Cross(normal, 1.0f); - float rtA = Vec2.Cross(ccp->RA, tangent); - float rtB = Vec2.Cross(ccp->RB, tangent); - rtA *= rtA; - rtB *= rtB; + float rt1 = Vec2.Cross(ccp.R1, tangent); + float rt2 = Vec2.Cross(ccp.R2, tangent); + rt1 *= rt1; + rt2 *= rt2; - float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB; + float kTangent = b1._invMass + b2._invMass + b1._invI * rt1 + b2._invI * rt2; - Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON); - ccp->TangentMass = 1.0f / kTangent; + Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON); + ccp.TangentMass = 1.0f / kTangent; - // Setup a velocity bias for restitution. - ccp->VelocityBias = 0.0f; - float vRel = Vec2.Dot(cc.Normal, vB + Vec2.Cross(wB, ccp->RB) - vA - Vec2.Cross(wA, ccp->RA)); - if (vRel < -Common.Settings.VelocityThreshold) + // Setup a velocity bias for restitution. + ccp.VelocityBias = 0.0f; + if (ccp.Separation > 0.0f) + { + ccp.VelocityBias = -step.Inv_Dt * ccp.Separation; // TODO_ERIN b2TimeStep + } + else + { + float vRel = Vec2.Dot(cc.Normal, v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1)); + if (vRel < -Settings.VelocityThreshold) { - ccp->VelocityBias = -cc.Restitution * vRel; + ccp.VelocityBias = -cc.Restitution * vRel; } } + } - // If we have two points, then prepare the block solver. - if (cc.PointCount == 2) + // If we have two points, then prepare the block solver. + if (cc.PointCount == 2) + { + ContactConstraintPoint ccp1 = cc.Points[0]; + ContactConstraintPoint ccp2 = cc.Points[1]; + + float invMass1 = b1._invMass; + float invI1 = b1._invI; + float invMass2 = b2._invMass; + float invI2 = b2._invI; + + float rn11 = Vec2.Cross(ccp1.R1, normal); + float rn12 = Vec2.Cross(ccp1.R2, normal); + float rn21 = Vec2.Cross(ccp2.R1, normal); + float rn22 = Vec2.Cross(ccp2.R2, normal); + + float k11 = invMass1 + invMass2 + invI1 * rn11 * rn11 + invI2 * rn12 * rn12; + float k22 = invMass1 + invMass2 + invI1 * rn21 * rn21 + invI2 * rn22 * rn22; + float k12 = invMass1 + invMass2 + invI1 * rn11 * rn21 + invI2 * rn12 * rn22; + + // Ensure a reasonable condition number. + const float k_maxConditionNumber = 100.0f; + if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { - ContactConstraintPoint* ccp1 = &ccPointsPtr[0]; - ContactConstraintPoint* ccp2 = &ccPointsPtr[1]; - - float invMassA = bodyA._invMass; - float invIA = bodyA._invI; - float invMassB = bodyB._invMass; - float invIB = bodyB._invI; - - float rn1A = Vec2.Cross(ccp1->RA, cc.Normal); - float rn1B = Vec2.Cross(ccp1->RB, cc.Normal); - float rn2A = Vec2.Cross(ccp2->RA, cc.Normal); - float rn2B = Vec2.Cross(ccp2->RB, cc.Normal); - - float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; - float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; - float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; - - // Ensure a reasonable condition number. - const float 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(); - } - else - { - // The constraints are redundant, just use one. - // TODO_ERIN use deepest? - cc.PointCount = 1; - } + // K is safe to invert. + cc.K.Col1.Set(k11, k12); + cc.K.Col2.Set(k12, k22); + cc.NormalMass = cc.K.Invert(); + } + else + { + // The constraints are redundant, just use one. + // TODO_ERIN use deepest? + cc.PointCount = 1; } } + + ++count; } } + + Box2DXDebug.Assert(count == _constraintCount); } public void Dispose() @@ -223,47 +236,41 @@ namespace Box2DX.Dynamics public void InitVelocityConstraints(TimeStep step) { - unsafe + // Warm start. + for (int i = 0; i < _constraintCount; ++i) { - // Warm start. - for (int i = 0; i < _constraintCount; ++i) + ContactConstraint c = _constraints[i]; + + Body b1 = c.Body1; + Body b2 = c.Body2; + float invMass1 = b1._invMass; + float invI1 = b1._invI; + float invMass2 = b2._invMass; + float invI2 = b2._invI; + Vec2 normal = c.Normal; + Vec2 tangent = Vec2.Cross(normal, 1.0f); + + if (step.WarmStarting) { - ContactConstraint c = _constraints[i]; - - Body bodyA = c.BodyA; - Body bodyB = c.BodyB; - float invMassA = bodyA._invMass; - float invIA = bodyA._invI; - float invMassB = bodyB._invMass; - float invIB = bodyB._invI; - Vec2 normal = c.Normal; - Vec2 tangent = Vec2.Cross(normal, 1.0f); - - fixed (ContactConstraintPoint* pointsPtr = c.Points) + for (int j = 0; j < c.PointCount; ++j) { - if (step.WarmStarting) - { - for (int j = 0; j < c.PointCount; ++j) - { - ContactConstraintPoint* ccp = &pointsPtr[j]; - ccp->NormalImpulse *= step.DtRatio; - ccp->TangentImpulse *= step.DtRatio; - Vec2 P = ccp->NormalImpulse * normal + ccp->TangentImpulse * tangent; - bodyA._angularVelocity -= invIA * Vec2.Cross(ccp->RA, P); - bodyA._linearVelocity -= invMassA * P; - bodyB._angularVelocity += invIB * Vec2.Cross(ccp->RB, P); - bodyB._linearVelocity += invMassB * P; - } - } - else - { - for (int j = 0; j < c.PointCount; ++j) - { - ContactConstraintPoint* ccp = &pointsPtr[j]; - ccp->NormalImpulse = 0.0f; - ccp->TangentImpulse = 0.0f; - } - } + ContactConstraintPoint ccp = c.Points[j]; + ccp.NormalImpulse *= step.DtRatio; + ccp.TangentImpulse *= step.DtRatio; + Vec2 P = ccp.NormalImpulse * normal + ccp.TangentImpulse * tangent; + b1._angularVelocity -= invI1 * Vec2.Cross(ccp.R1, P); + b1._linearVelocity -= invMass1 * P; + b2._angularVelocity += invI2 * Vec2.Cross(ccp.R2, P); + b2._linearVelocity += invMass2 * P; + } + } + else + { + for (int j = 0; j < c.PointCount; ++j) + { + ContactConstraintPoint ccp = c.Points[j]; + ccp.NormalImpulse = 0.0f; + ccp.TangentImpulse = 0.0f; } } } @@ -274,303 +281,297 @@ namespace Box2DX.Dynamics for (int i = 0; i < _constraintCount; ++i) { ContactConstraint c = _constraints[i]; - Body bodyA = c.BodyA; - Body bodyB = c.BodyB; - float wA = bodyA._angularVelocity; - float wB = bodyB._angularVelocity; - Vec2 vA = bodyA._linearVelocity; - Vec2 vB = bodyB._linearVelocity; - float invMassA = bodyA._invMass; - float invIA = bodyA._invI; - float invMassB = bodyB._invMass; - float invIB = bodyB._invI; + Body b1 = c.Body1; + Body b2 = c.Body2; + float w1 = b1._angularVelocity; + float w2 = b2._angularVelocity; + Vec2 v1 = b1._linearVelocity; + Vec2 v2 = b2._linearVelocity; + float invMass1 = b1._invMass; + float invI1 = b1._invI; + float invMass2 = b2._invMass; + float invI2 = b2._invI; Vec2 normal = c.Normal; Vec2 tangent = Vec2.Cross(normal, 1.0f); float friction = c.Friction; Box2DXDebug.Assert(c.PointCount == 1 || c.PointCount == 2); - unsafe + // Solve normal constraints + if (c.PointCount == 1) { - fixed (ContactConstraintPoint* pointsPtr = c.Points) + ContactConstraintPoint ccp = c.Points[0]; + + // Relative velocity at contact + Vec2 dv = v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1); + + // Compute normal impulse + float vn = Vec2.Dot(dv, normal); + float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias); + + // Clamp the accumulated impulse + float newImpulse = Common.Math.Max(ccp.NormalImpulse + lambda, 0.0f); + lambda = newImpulse - ccp.NormalImpulse; + + // Apply contact impulse + Vec2 P = lambda * normal; + + v1 -= invMass1 * P; + w1 -= invI1 * Vec2.Cross(ccp.R1, P); + + v2 += invMass2 * P; + w2 += invI2 * Vec2.Cross(ccp.R2, P); + + ccp.NormalImpulse = newImpulse; + } + else + { + // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite). + // Build the mini LCP for this contact patch + // + // 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 + // + // 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 + // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid + // solution that satisfies the problem is chosen. + // + // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires + // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i). + // + // Substitute: + // + // x = x' - a + // + // Plug into above equation: + // + // vn = A * x + b + // = A * (x' - a) + b + // = A * x' + b - A * a + // = A * x' + b' + // b' = b - A * a; + + ContactConstraintPoint cp1 = c.Points[0]; + ContactConstraintPoint cp2 = c.Points[1]; + + Vec2 a = new Vec2(cp1.NormalImpulse, cp2.NormalImpulse); + Box2DXDebug.Assert(a.X >= 0.0f && a.Y >= 0.0f); + + // Relative velocity at contact + Vec2 dv1 = v2 + Vec2.Cross(w2, cp1.R2) - v1 - Vec2.Cross(w1, cp1.R1); + Vec2 dv2 = v2 + Vec2.Cross(w2, cp2.R2) - v1 - Vec2.Cross(w1, cp2.R1); + + // Compute normal velocity + float vn1 = Vec2.Dot(dv1, normal); + float vn2 = Vec2.Dot(dv2, normal); + + Vec2 b; + b.X = vn1 - cp1.VelocityBias; + b.Y = vn2 - cp2.VelocityBias; + b -= Common.Math.Mul(c.K, a); + + const float k_errorTol = 1e-3f; + for (; ; ) { - // Solve tangent constraints - for (int j = 0; j < c.PointCount; ++j) + // + // Case 1: vn = 0 + // + // 0 = A * x' + b' + // + // Solve for x': + // + // x' = - inv(A) * b' + // + Vec2 x = -Common.Math.Mul(c.NormalMass, b); + + if (x.X >= 0.0f && x.Y >= 0.0f) { - ContactConstraintPoint* ccp = &pointsPtr[j]; - - // Relative velocity at contact - Vec2 dv = vB + Vec2.Cross(wB, ccp->RB) - vA - Vec2.Cross(wA, ccp->RA); + // Resubstitute for the incremental impulse + Vec2 d = x - a; - // Compute tangent force - float vt = Vec2.Dot(dv, tangent); - float lambda = ccp->TangentMass * (-vt); + // Apply incremental impulse + Vec2 P1 = d.X * normal; + Vec2 P2 = d.Y * normal; + v1 -= invMass1 * (P1 + P2); + w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2)); - // b2Clamp the accumulated force - float maxFriction = friction * ccp->NormalImpulse; - float newImpulse = Common.Math.Clamp(ccp->TangentImpulse + lambda, -maxFriction, maxFriction); - lambda = newImpulse - ccp->TangentImpulse; + v2 += invMass2 * (P1 + P2); + w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2)); - // Apply contact impulse - Vec2 P = lambda * tangent; + // Accumulate + cp1.NormalImpulse = x.X; + cp2.NormalImpulse = x.Y; - vA -= invMassA * P; - wA -= invIA * Vec2.Cross(ccp->RA, P); +#if B2_DEBUG_SOLVER + // Postconditions + dv1 = v2 + Vector2.Cross(w2, cp1.R2) - v1 - Vector2.Cross(w1, cp1.R1); + dv2 = v2 + Vector2.Cross(w2, cp2.R2) - v1 - Vector2.Cross(w1, cp2.R1); - vB += invMassB * P; - wB += invIB * Vec2.Cross(ccp->RB, P); + // Compute normal velocity + vn1 = Vector2.Dot(dv1, normal); + vn2 = Vector2.Dot(dv2, normal); - ccp->TangentImpulse = newImpulse; + Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol); + Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol); +#endif + break; } - // Solve normal constraints - if (c.PointCount == 1) + // + // Case 2: vn1 = 0 and x2 = 0 + // + // 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; + + if (x.X >= 0.0f && vn2 >= 0.0f) { - ContactConstraintPoint ccp = c.Points[0]; + // Resubstitute for the incremental impulse + Vec2 d = x - a; - // Relative velocity at contact - Vec2 dv = vB + Vec2.Cross(wB, ccp.RB) - vA - Vec2.Cross(wA, ccp.RA); + // Apply incremental impulse + Vec2 P1 = d.X * normal; + Vec2 P2 = d.Y * normal; + v1 -= invMass1 * (P1 + P2); + w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2)); - // Compute normal impulse - float vn = Vec2.Dot(dv, normal); - float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias); + v2 += invMass2 * (P1 + P2); + w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2)); - // Clamp the accumulated impulse - float newImpulse = Common.Math.Max(ccp.NormalImpulse + lambda, 0.0f); - lambda = newImpulse - ccp.NormalImpulse; + // Accumulate + cp1.NormalImpulse = x.X; + cp2.NormalImpulse = x.Y; - // Apply contact impulse - Vec2 P = lambda * normal; - vA -= invMassA * P; - wA -= invIA * Vec2.Cross(ccp.RA, P); +#if B2_DEBUG_SOLVER + // Postconditions + dv1 = v2 + Vector2.Cross(w2, cp1.R2) - v1 - Vector2.Cross(w1, cp1.R1); - vB += invMassB * P; - wB += invIB * Vec2.Cross(ccp.RB, P); - ccp.NormalImpulse = newImpulse; + // Compute normal velocity + vn1 = Vector2.Dot(dv1, normal); + + Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol); +#endif + break; } - else + + + // + // Case 3: w2 = 0 and x1 = 0 + // + // 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; + vn2 = 0.0f; + + if (x.Y >= 0.0f && vn1 >= 0.0f) { - // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite). - // Build the mini LCP for this contact patch - // - // 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 - // - // 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 - // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid - // solution that satisfies the problem is chosen. - // - // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires - // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i). - // - // Substitute: - // - // x = x' - a - // - // Plug into above equation: - // - // vn = A * x + b - // = A * (x' - a) + b - // = A * x' + b - A * a - // = A * x' + b' - // b' = b - A * a; - - ContactConstraintPoint* cp1 = &pointsPtr[0]; - ContactConstraintPoint* cp2 = &pointsPtr[1]; - - Vec2 a = new Vec2(cp1->NormalImpulse, cp2->NormalImpulse); - Box2DXDebug.Assert(a.X >= 0.0f && a.Y >= 0.0f); - - // Relative velocity at contact - Vec2 dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA); - Vec2 dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA); + // Resubstitute for the incremental impulse + Vec2 d = x - a; - // Compute normal velocity - float vn1 = Vec2.Dot(dv1, normal); - float vn2 = Vec2.Dot(dv2, normal); + // Apply incremental impulse + Vec2 P1 = d.X * normal; + Vec2 P2 = d.Y * normal; + v1 -= invMass1 * (P1 + P2); + w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2)); - Vec2 b; - b.X = vn1 - cp1->VelocityBias; - b.Y = vn2 - cp2->VelocityBias; - b -= Common.Math.Mul(c.K, a); + v2 += invMass2 * (P1 + P2); + w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2)); - const float k_errorTol = 1e-3f; - //B2_NOT_USED(k_errorTol); + // Accumulate + cp1.NormalImpulse = x.X; + cp2.NormalImpulse = x.Y; - for (; ; ) - { - // - // Case 1: vn = 0 - // - // 0 = A * x' + b' - // - // Solve for x': - // - // x' = - inv(A) * b' - // - Vec2 x = -Common.Math.Mul(c.NormalMass, b); - - if (x.X >= 0.0f && x.Y >= 0.0f) - { - // Resubstitute for the incremental impulse - Vec2 d = x - a; - - // Apply incremental impulse - Vec2 P1 = d.X * normal; - Vec2 P2 = d.Y * normal; - vA -= invMassA * (P1 + P2); - wA -= invIA * (Vec2.Cross(cp1->RA, P1) + Vec2.Cross(cp2->RA, P2)); - - vB += invMassB * (P1 + P2); - wB += invIB * (Vec2.Cross(cp1->RB, P1) + Vec2.Cross(cp2->RB, P2)); - - // Accumulate - cp1->NormalImpulse = x.X; - cp2->NormalImpulse = x.Y; - -#if DEBUG_SOLVER - // Postconditions - dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA); - dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA); - - // Compute normal velocity - vn1 = Vec2.Dot(dv1, normal); - vn2 = Vec2.Dot(dv2, normal); - - Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol); - Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol); -#endif - break; - } - - // - // Case 2: vn1 = 0 and x2 = 0 - // - // 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; - - if (x.X >= 0.0f && vn2 >= 0.0f) - { - // Resubstitute for the incremental impulse - Vec2 d = x - a; - - // Apply incremental impulse - Vec2 P1 = d.X * normal; - Vec2 P2 = d.Y * normal; - vA -= invMassA * (P1 + P2); - wA -= invIA * (Vec2.Cross(cp1->RA, P1) + Vec2.Cross(cp2->RA, P2)); - - vB += invMassB * (P1 + P2); - wB += invIB * (Vec2.Cross(cp1->RB, P1) + Vec2.Cross(cp2->RB, P2)); - - // Accumulate - cp1->NormalImpulse = x.X; - cp2->NormalImpulse = x.Y; - -#if DEBUG_SOLVER - // Postconditions - dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA); - - // Compute normal velocity - vn1 = Vec2.Dot(dv1, normal); - - Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol); -#endif - break; - } - - - // - // Case 3: w2 = 0 and x1 = 0 - // - // 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; - vn2 = 0.0f; - - if (x.Y >= 0.0f && vn1 >= 0.0f) - { - // Resubstitute for the incremental impulse - Vec2 d = x - a; - - // Apply incremental impulse - Vec2 P1 = d.X * normal; - Vec2 P2 = d.Y * normal; - vA -= invMassA * (P1 + P2); - wA -= invIA * (Vec2.Cross(cp1->RA, P1) + Vec2.Cross(cp2->RA, P2)); - - vB += invMassB * (P1 + P2); - wB += invIB * (Vec2.Cross(cp1->RB, P1) + Vec2.Cross(cp2->RB, P2)); - - // Accumulate - cp1->NormalImpulse = x.X; - cp2->NormalImpulse = x.Y; - -#if DEBUG_SOLVER - // Postconditions - dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA); - - // Compute normal velocity - vn2 = Vec2.Dot(dv2, normal); - - Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol); +#if B2_DEBUG_SOLVER + // Postconditions + dv2 = v2 + Vector2.Cross(w2, cp2.R2) - v1 - Vector2.Cross(w1, cp2.R1); + + // Compute normal velocity + vn2 = Vector2.Dot(dv2, normal); + + Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol); #endif - break; - } - - // - // Case 4: x1 = 0 and x2 = 0 - // - // vn1 = b1 - // vn2 = b2; - x.X = 0.0f; - x.Y = 0.0f; - vn1 = b.X; - vn2 = b.Y; - - if (vn1 >= 0.0f && vn2 >= 0.0f) - { - // Resubstitute for the incremental impulse - Vec2 d = x - a; - - // Apply incremental impulse - Vec2 P1 = d.X * normal; - Vec2 P2 = d.Y * normal; - vA -= invMassA * (P1 + P2); - wA -= invIA * (Vec2.Cross(cp1->RA, P1) + Vec2.Cross(cp2->RA, P2)); - - vB += invMassB * (P1 + P2); - wB += invIB * (Vec2.Cross(cp1->RB, P1) + Vec2.Cross(cp2->RB, P2)); - - // Accumulate - cp1->NormalImpulse = x.X; - cp2->NormalImpulse = x.Y; - - break; - } - - // No solution, give up. This is hit sometimes, but it doesn't seem to matter. - break; - } + break; + } + + // + // Case 4: x1 = 0 and x2 = 0 + // + // vn1 = b1 + // vn2 = b2; + x.X = 0.0f; + x.Y = 0.0f; + vn1 = b.X; + vn2 = b.Y; + + if (vn1 >= 0.0f && vn2 >= 0.0f) + { + // Resubstitute for the incremental impulse + Vec2 d = x - a; + + // Apply incremental impulse + Vec2 P1 = d.X * normal; + Vec2 P2 = d.Y * normal; + v1 -= invMass1 * (P1 + P2); + w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2)); + + v2 += invMass2 * (P1 + P2); + w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2)); + + // Accumulate + cp1.NormalImpulse = x.X; + cp2.NormalImpulse = x.Y; + + break; } - bodyA._linearVelocity = vA; - bodyA._angularVelocity = wA; - bodyB._linearVelocity = vB; - bodyB._angularVelocity = wB; + // No solution, give up. This is hit sometimes, but it doesn't seem to matter. + break; } } + + // Solve tangent constraints + for (int j = 0; j < c.PointCount; ++j) + { + ContactConstraintPoint ccp = c.Points[j]; + + // Relative velocity at contact + Vec2 dv = v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1); + + // Compute tangent force + float vt = Vec2.Dot(dv, tangent); + float lambda = ccp.TangentMass * (-vt); + + // Clamp the accumulated force + float maxFriction = friction * ccp.NormalImpulse; + float newImpulse = Common.Math.Clamp(ccp.TangentImpulse + lambda, -maxFriction, maxFriction); + lambda = newImpulse - ccp.TangentImpulse; + + // Apply contact impulse + Vec2 P = lambda * tangent; + + v1 -= invMass1 * P; + w1 -= invI1 * Vec2.Cross(ccp.R1, P); + + v2 += invMass2 * P; + w2 += invI2 * Vec2.Cross(ccp.R2, P); + + ccp.TangentImpulse = newImpulse; + } + + b1._linearVelocity = v1; + b1._angularVelocity = w1; + b2._linearVelocity = v2; + b2._angularVelocity = w2; } } @@ -589,73 +590,7 @@ namespace Box2DX.Dynamics } } - internal class PositionSolverManifold - { - internal Vec2 Normal; - internal Vec2[] Points = new Vec2[Settings.MaxManifoldPoints]; - internal float[] Separations = new float[Settings.MaxManifoldPoints]; - - internal void Initialize(ContactConstraint cc) - { - Box2DXDebug.Assert(cc.PointCount > 0); - - switch (cc.Type) - { - case ManifoldType.Circles: - { - Vec2 pointA = cc.BodyA.GetWorldPoint(cc.LocalPoint); - Vec2 pointB = cc.BodyB.GetWorldPoint(cc.Points[0].LocalPoint); - if (Vec2.DistanceSquared(pointA, pointB) > Settings.FLT_EPSILON_SQUARED) - { - Normal = pointB - pointA; - Normal.Normalize(); - } - else - { - Normal.Set(1.0f, 0.0f); - } - - Points[0] = 0.5f * (pointA + pointB); - Separations[0] = Vec2.Dot(pointB - pointA, Normal) - cc.Radius; - } - break; - - case ManifoldType.FaceA: - { - Normal = cc.BodyA.GetWorldVector(cc.LocalPlaneNormal); - Vec2 planePoint = cc.BodyA.GetWorldPoint(cc.LocalPoint); - - for (int i = 0; i < cc.PointCount; ++i) - { - Vec2 clipPoint = cc.BodyB.GetWorldPoint(cc.Points[i].LocalPoint); - Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius; - Points[i] = clipPoint; - } - } - break; - - case ManifoldType.FaceB: - { - Normal = cc.BodyB.GetWorldVector(cc.LocalPlaneNormal); - Vec2 planePoint = cc.BodyB.GetWorldPoint(cc.LocalPoint); - - for (int i = 0; i < cc.PointCount; ++i) - { - Vec2 clipPoint = cc.BodyA.GetWorldPoint(cc.Points[i].LocalPoint); - Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius; - Points[i] = clipPoint; - } - - // Ensure normal points from A to B - Normal = -Normal; - } - break; - } - } - } - - private static PositionSolverManifold s_PositionSolverManifold = new PositionSolverManifold(); - +#if FIRST public bool SolvePositionConstraints(float baumgarte) { float minSeparation = 0.0f; @@ -663,25 +598,29 @@ namespace Box2DX.Dynamics for (int i = 0; i < _constraintCount; ++i) { ContactConstraint c = _constraints[i]; - Body bodyA = c.BodyA; - Body bodyB = c.BodyB; - - float invMassA = bodyA._mass * bodyA._invMass; - float invIA = bodyA._mass * bodyA._invI; - float invMassB = bodyB._mass * bodyB._invMass; - float invIB = bodyB._mass * bodyB._invI; + Body b1 = c.Body1; + Body b2 = c.Body2; + float invMass1 = b1._mass * b1._invMass; + float invI1 = b1._mass * b1._invI; + float invMass2 = b2._mass * b2._invMass; + float invI2 = b2._mass * b2._invI; - s_PositionSolverManifold.Initialize(c); - Vec2 normal = s_PositionSolverManifold.Normal; + Vec2 normal = c.Normal; // Solver normal constraints for (int j = 0; j < c.PointCount; ++j) { - Vec2 point = s_PositionSolverManifold.Points[j]; - float separation = s_PositionSolverManifold.Separations[j]; + ContactConstraintPoint ccp = c.Points[j]; + + Vec2 r1 = Common.Math.Mul(b1.GetXForm().R, ccp.LocalAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Common.Math.Mul(b2.GetXForm().R, ccp.LocalAnchor2 - b2.GetLocalCenter()); - Vec2 rA = point - bodyA._sweep.C; - Vec2 rB = point - bodyB._sweep.C; + Vec2 p1 = b1._sweep.C + r1; + Vec2 p2 = b2._sweep.C + r2; + Vec2 dp = p2 - p1; + + // Approximate the current separation. + float separation = Vec2.Dot(dp, normal) + ccp.Separation; // Track max constraint error. minSeparation = Common.Math.Min(minSeparation, separation); @@ -690,17 +629,17 @@ namespace Box2DX.Dynamics float C = baumgarte * Common.Math.Clamp(separation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); // Compute normal impulse - float impulse = -c.Points[j].EqualizedMass * C; + float impulse = -ccp.EqualizedMass * C; Vec2 P = impulse * normal; - bodyA._sweep.C -= invMassA * P; - bodyA._sweep.A -= invIA * Vec2.Cross(rA, P); - bodyA.SynchronizeTransform(); + b1._sweep.C -= invMass1 * P; + b1._sweep.A -= invI1 * Vec2.Cross(r1, P); + b1.SynchronizeTransform(); - bodyB._sweep.C += invMassB * P; - bodyB._sweep.A += invIB * Vec2.Cross(rB, P); - bodyB.SynchronizeTransform(); + b2._sweep.C += invMass2 * P; + b2._sweep.A += invI2 * Vec2.Cross(r2, P); + b2.SynchronizeTransform(); } } @@ -709,4 +648,6 @@ namespace Box2DX.Dynamics return minSeparation >= -1.5f * Settings.LinearSlop; } } +#else +#endif } diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs deleted file mode 100644 index 999154c..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs +++ /dev/null @@ -1,55 +0,0 @@ -/* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 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. -*/ - -using Box2DX.Collision; -using Box2DX.Common; - -namespace Box2DX.Dynamics -{ - public class EdgeAndCircleContact : Contact - { - public EdgeAndCircleContact(Fixture fixtureA, Fixture fixtureB) - : base(fixtureA, fixtureB) - { - Box2DXDebug.Assert(fixtureA.ShapeType == ShapeType.EdgeShape); - Box2DXDebug.Assert(fixtureB.ShapeType == ShapeType.CircleShape); - _manifold.PointCount = 0; - _manifold.Points[0].NormalImpulse = 0.0f; - _manifold.Points[0].TangentImpulse = 0.0f; - CollideShapeFunction = CollideEdgeAndCircle; - } - - private static void CollideEdgeAndCircle(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) - { - Collision.Collision.CollideEdgeAndCircle(ref manifold, (EdgeShape)shape1, xf1, (CircleShape)shape2, xf2); - } - - new public static Contact Create(Fixture fixtureA, Fixture fixtureB) - { - return new EdgeAndCircleContact(fixtureA, fixtureB); - } - - new public static void Destroy(ref Contact contact) - { - contact = null; - } - } -} \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs.meta deleted file mode 100644 index 442c45d..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs.meta +++ /dev/null @@ -1,11 +0,0 @@ -fileFormatVersion: 2 -guid: 576caff6a1c4ee846aa596cfb70c8a29 -MonoImporter: - externalObjects: {} - serializedVersion: 2 - defaultReferences: [] - executionOrder: 0 - icon: {instanceID: 0} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/NullContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/NullContact.cs index 31c7bb1..2012997 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/NullContact.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/NullContact.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -19,6 +19,10 @@ 3. This notice may not be removed or altered from any source distribution. */ +using System; +using System.Collections.Generic; +using System.Text; + using Box2DX.Collision; using Box2DX.Common; @@ -26,10 +30,8 @@ namespace Box2DX.Dynamics { public class NullContact : Contact { - public NullContact() - { - CollideShapeFunction = Collide; - } - private static void Collide(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) { } + public NullContact() { } + public override void Evaluate(ContactListener listener) { } + public override Manifold[] GetManifolds() { return null; } } } \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndCircleContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndCircleContact.cs index 4a63bf8..4166623 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndCircleContact.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndCircleContact.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -19,6 +19,10 @@ 3. This notice may not be removed or altered from any source distribution. */ +using System; +using System.Collections.Generic; +using System.Text; + using Box2DX.Collision; using Box2DX.Common; @@ -26,25 +30,139 @@ namespace Box2DX.Dynamics { public class PolyAndCircleContact : Contact { - public PolyAndCircleContact(Fixture fixtureA, Fixture fixtureB) - : base(fixtureA, fixtureB) + public Manifold _manifold = new Manifold(); + + public override Manifold[] GetManifolds() + { + return new Manifold[] { _manifold }; + } + + public PolyAndCircleContact(Shape s1, Shape s2) + : base(s1, s2) { - Box2DXDebug.Assert(fixtureA.ShapeType == ShapeType.PolygonShape); - Box2DXDebug.Assert(fixtureB.ShapeType == ShapeType.CircleShape); - CollideShapeFunction = CollidePolygonCircle; + Box2DXDebug.Assert(_shape1.GetType() == ShapeType.PolygonShape); + Box2DXDebug.Assert(_shape2.GetType() == ShapeType.CircleShape); + _manifold.PointCount = 0; } - private static void CollidePolygonCircle(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) + public override void Evaluate(ContactListener listener) { - Collision.Collision.CollidePolygonAndCircle(ref manifold, (PolygonShape)shape1, xf1, (CircleShape)shape2, xf2); + Body b1 = _shape1.GetBody(); + Body b2 = _shape2.GetBody(); +#warning "needfix" + //memcpy(&m0, &m_manifold, sizeof(b2Manifold)); + Manifold m0 = _manifold.Clone(); + + Collision.Collision.CollidePolygonAndCircle(ref _manifold, (PolygonShape)_shape1, b1.GetXForm(), + (CircleShape)_shape2, b2.GetXForm()); + + bool[] persisted = new bool[] { false, false }; + + ContactPoint cp = new ContactPoint(); + cp.Shape1 = _shape1; + cp.Shape2 = _shape2; + cp.Friction = Settings.MixFriction(_shape1.Friction, _shape2.Friction); + cp.Restitution = Settings.MixRestitution(_shape1.Restitution, _shape2.Restitution); + + // Match contact ids to facilitate warm starting. + if (_manifold.PointCount > 0) + { + // Match old contact ids to new contact ids and copy the + // stored impulses to warm start the solver. + for (int i = 0; i < _manifold.PointCount; ++i) + { + ManifoldPoint mp = _manifold.Points[i]; + mp.NormalImpulse = 0.0f; + mp.TangentImpulse = 0.0f; + bool found = false; + ContactID id = mp.ID; + + for (int j = 0; j < m0.PointCount; ++j) + { + if (persisted[j] == true) + { + continue; + } + + ManifoldPoint mp0 = m0.Points[j]; + + if (mp0.ID.Key == id.Key) + { + persisted[j] = true; + mp.NormalImpulse = mp0.NormalImpulse; + mp.TangentImpulse = mp0.TangentImpulse; + + // A persistent point. + found = true; + + // Report persistent point. + if (listener != null) + { + cp.Position = b1.GetWorldPoint(mp.LocalPoint1); + Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1); + Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2); + cp.Velocity = v2 - v1; + cp.Normal = _manifold.Normal; + cp.Separation = mp.Separation; + cp.ID = id; + listener.Persist(cp); + } + break; + } + } + + // Report added point. + if (found == false && listener != null) + { + cp.Position = b1.GetWorldPoint(mp.LocalPoint1); + Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1); + Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2); + cp.Velocity = v2 - v1; + cp.Normal = _manifold.Normal; + cp.Separation = mp.Separation; + cp.ID = id; + listener.Add(cp); + } + } + + _manifoldCount = 1; + } + else + { + _manifoldCount = 0; + } + + if (listener == null) + { + return; + } + + // Report removed points. + for (int i = 0; i < m0.PointCount; ++i) + { + if (persisted[i]) + { + continue; + } + + ManifoldPoint mp0 = m0.Points[i]; + cp.Position = b1.GetWorldPoint(mp0.LocalPoint1); + Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp0.LocalPoint1); + Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp0.LocalPoint2); + cp.Velocity = v2 - v1; + cp.Normal = m0.Normal; + cp.Separation = mp0.Separation; + cp.ID = mp0.ID; + listener.Remove(cp); + } } - new public static Contact Create(Fixture fixtureA, Fixture fixtureB) + new public static Contact Create(Shape shape1, Shape shape2) { - return new PolyAndCircleContact(fixtureA, fixtureB); + return new PolyAndCircleContact(shape1, shape2); } - new public static void Destroy(ref Contact contact) + new public static void Destroy(Contact contact) { contact = null; } diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs deleted file mode 100644 index a50553f..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs +++ /dev/null @@ -1,52 +0,0 @@ -/* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 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. -*/ - -using Box2DX.Collision; -using Box2DX.Common; - -namespace Box2DX.Dynamics -{ - public class PolyAndEdgeContact : Contact - { - public PolyAndEdgeContact(Fixture fixtureA, Fixture fixtureB) - : base(fixtureA, fixtureB) - { - Box2DXDebug.Assert(fixtureA.ShapeType == ShapeType.PolygonShape); - Box2DXDebug.Assert(fixtureB.ShapeType == ShapeType.EdgeShape); - CollideShapeFunction = CollidePolyAndEdgeContact; - } - - private static void CollidePolyAndEdgeContact(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) - { - Collision.Collision.CollidePolyAndEdge(ref manifold, (PolygonShape)shape1, xf1, (EdgeShape)shape2, xf2); - } - - new public static Contact Create(Fixture fixtureA, Fixture fixtureB) - { - return new PolyAndEdgeContact(fixtureA, fixtureB); - } - - new public static void Destroy(ref Contact contact) - { - contact = null; - } - } -} \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs.meta deleted file mode 100644 index 5f83e8a..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs.meta +++ /dev/null @@ -1,11 +0,0 @@ -fileFormatVersion: 2 -guid: c42cbf67f18e2414192739161ff94e89 -MonoImporter: - externalObjects: {} - serializedVersion: 2 - defaultReferences: [] - executionOrder: 0 - icon: {instanceID: 0} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyContact.cs index 5b101fb..ebf24cf 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyContact.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyContact.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -19,6 +19,10 @@ 3. This notice may not be removed or altered from any source distribution. */ +using System; +using System.Collections.Generic; +using System.Text; + using Box2DX.Collision; using Box2DX.Common; @@ -26,25 +30,139 @@ namespace Box2DX.Dynamics { public class PolygonContact : Contact { - public PolygonContact(Fixture fixtureA, Fixture fixtureB) - : base(fixtureA, fixtureB) + public Manifold _manifold = new Manifold(); + + public override Manifold[] GetManifolds() + { + return new Manifold[] { _manifold }; + } + + public PolygonContact(Shape s1, Shape s2) + : base(s1, s2) { - Box2DXDebug.Assert(fixtureA.ShapeType == ShapeType.PolygonShape); - Box2DXDebug.Assert(fixtureB.ShapeType == ShapeType.PolygonShape); - CollideShapeFunction = CollidePolygons; + Box2DXDebug.Assert(_shape1.GetType() == ShapeType.PolygonShape); + Box2DXDebug.Assert(_shape2.GetType() == ShapeType.PolygonShape); + _manifold.PointCount = 0; } - private static void CollidePolygons(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) + public override void Evaluate(ContactListener listener) { - Collision.Collision.CollidePolygons(ref manifold, (PolygonShape)shape1, xf1, (PolygonShape)shape2, xf2); + Body b1 = _shape1.GetBody(); + Body b2 = _shape2.GetBody(); +#warning "needfix" + //memcpy(&m0, &m_manifold, sizeof(b2Manifold)); + Manifold m0 = _manifold.Clone(); + + Collision.Collision.CollidePolygons(ref _manifold, (PolygonShape)_shape1, b1.GetXForm(), + (PolygonShape)_shape2, b2.GetXForm()); + + bool[] persisted = new bool[] { false, false }; + + ContactPoint cp = new ContactPoint(); + cp.Shape1 = _shape1; + cp.Shape2 = _shape2; + cp.Friction = Settings.MixFriction(_shape1.Friction, _shape2.Friction); + cp.Restitution = Settings.MixRestitution(_shape1.Restitution, _shape2.Restitution); + + // Match contact ids to facilitate warm starting. + if (_manifold.PointCount > 0) + { + // Match old contact ids to new contact ids and copy the + // stored impulses to warm start the solver. + for (int i = 0; i < _manifold.PointCount; ++i) + { + ManifoldPoint mp = _manifold.Points[i]; + mp.NormalImpulse = 0.0f; + mp.TangentImpulse = 0.0f; + bool found = false; + ContactID id = mp.ID; + + for (int j = 0; j < m0.PointCount; ++j) + { + if (persisted[j] == true) + { + continue; + } + + ManifoldPoint mp0 = m0.Points[j]; + + if (mp0.ID.Key == id.Key) + { + persisted[j] = true; + mp.NormalImpulse = mp0.NormalImpulse; + mp.TangentImpulse = mp0.TangentImpulse; + + // A persistent point. + found = true; + + // Report persistent point. + if (listener != null) + { + cp.Position = b1.GetWorldPoint(mp.LocalPoint1); + Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1); + Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2); + cp.Velocity = v2 - v1; + cp.Normal = _manifold.Normal; + cp.Separation = mp.Separation; + cp.ID = id; + listener.Persist(cp); + } + break; + } + } + + // Report added point. + if (found == false && listener != null) + { + cp.Position = b1.GetWorldPoint(mp.LocalPoint1); + Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1); + Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2); + cp.Velocity = v2 - v1; + cp.Normal = _manifold.Normal; + cp.Separation = mp.Separation; + cp.ID = id; + listener.Add(cp); + } + } + + _manifoldCount = 1; + } + else + { + _manifoldCount = 0; + } + + if (listener == null) + { + return; + } + + // Report removed points. + for (int i = 0; i < m0.PointCount; ++i) + { + if (persisted[i]) + { + continue; + } + + ManifoldPoint mp0 = m0.Points[i]; + cp.Position = b1.GetWorldPoint(mp0.LocalPoint1); + Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp0.LocalPoint1); + Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp0.LocalPoint2); + cp.Velocity = v2 - v1; + cp.Normal = m0.Normal; + cp.Separation = mp0.Separation; + cp.ID = mp0.ID; + listener.Remove(cp); + } } - new public static Contact Create(Fixture fixtureA, Fixture fixtureB) + new public static Contact Create(Shape shape1, Shape shape2) { - return new PolygonContact(fixtureA, fixtureB); + return new PolygonContact(shape1, shape2); } - new public static void Destroy(ref Contact contact) + new public static void Destroy(Contact contact) { contact = null; } diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers.meta deleted file mode 100644 index 446ed50..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers.meta +++ /dev/null @@ -1,8 +0,0 @@ -fileFormatVersion: 2 -guid: 960c2a03774a86d4986f403eb182daaa -folderAsset: yes -DefaultImporter: - externalObjects: {} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs deleted file mode 100644 index dcdb244..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs +++ /dev/null @@ -1,156 +0,0 @@ -using Box2DX.Common; - -namespace Box2DX.Dynamics.Controllers -{ - /// - /// This class is used to build buoyancy controllers - /// - public class BuoyancyControllerDef - { - /// The outer surface normal - public Vec2 Normal; - /// The height of the fluid surface along the normal - public float Offset; - /// The fluid density - public float Density; - /// Fluid velocity, for drag calculations - public Vec2 Velocity; - /// Linear drag co-efficient - public float LinearDrag; - /// Linear drag co-efficient - public float AngularDrag; - /// If false, bodies are assumed to be uniformly dense, otherwise use the shapes densities - public bool UseDensity; //False by default to prevent a gotcha - /// If true, gravity is taken from the world instead of the gravity parameter. - public bool UseWorldGravity; - /// Gravity vector, if the world's gravity is not used - public Vec2 Gravity; - - public BuoyancyControllerDef() - { - Normal = new Vec2(0, 1); - Offset = 0; - Density = 0; - Velocity = new Vec2(0, 0); - LinearDrag = 0; - AngularDrag = 0; - UseDensity = false; - UseWorldGravity = true; - Gravity = new Vec2(0, 0); - } - } - - /// - /// Calculates buoyancy forces for fluids in the form of a half plane. - /// - public class BuoyancyController : Controller - { - /// The outer surface normal - public Vec2 Normal; - /// The height of the fluid surface along the normal - public float Offset; - /// The fluid density - public float Density; - /// Fluid velocity, for drag calculations - public Vec2 Velocity; - /// Linear drag co-efficient - public float LinearDrag; - /// Linear drag co-efficient - public float AngularDrag; - /// If false, bodies are assumed to be uniformly dense, otherwise use the shapes densities - public bool UseDensity; //False by default to prevent a gotcha - /// If true, gravity is taken from the world instead of the gravity parameter. - public bool UseWorldGravity; - /// Gravity vector, if the world's gravity is not used - public Vec2 Gravity; - - public BuoyancyController(BuoyancyControllerDef buoyancyControllerDef) - { - Normal = buoyancyControllerDef.Normal; - Offset = buoyancyControllerDef.Offset; - Density = buoyancyControllerDef.Density; - Velocity = buoyancyControllerDef.Velocity; - LinearDrag = buoyancyControllerDef.LinearDrag; - AngularDrag = buoyancyControllerDef.AngularDrag; - UseDensity = buoyancyControllerDef.UseDensity; - UseWorldGravity = buoyancyControllerDef.UseWorldGravity; - Gravity = buoyancyControllerDef.Gravity; - } - - public override void Step(TimeStep step) - { - //B2_NOT_USED(step); - if (_bodyList == null) - return; - - if (UseWorldGravity) - { - Gravity = _world.Gravity; - } - for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) - { - Body body = i.body; - if (body.IsSleeping()) - { - //Buoyancy force is just a function of position, - //so unlike most forces, it is safe to ignore sleeping bodes - continue; - } - Vec2 areac = new Vec2(0, 0); - Vec2 massc = new Vec2(0, 0); - float area = 0; - float mass = 0; - for (Fixture shape = body.GetFixtureList(); shape != null; shape = shape.Next) - { - Vec2 sc; - float sarea = shape.ComputeSubmergedArea(Normal, Offset, out sc); - area += sarea; - areac.X += sarea * sc.X; - areac.Y += sarea * sc.Y; - float shapeDensity = 0; - if (UseDensity) - { - //TODO: Expose density publicly - shapeDensity = shape.Density; - } - else - { - shapeDensity = 1; - } - mass += sarea * shapeDensity; - massc.X += sarea * sc.X * shapeDensity; - massc.Y += sarea * sc.Y * shapeDensity; - } - areac.X /= area; - areac.Y /= area; - //Vec2 localCentroid = Math.MulT(body.GetXForm(), areac); - massc.X /= mass; - massc.Y /= mass; - if (area < Settings.FLT_EPSILON) - continue; - //Buoyancy - Vec2 buoyancyForce = -Density * area * Gravity; - body.ApplyForce(buoyancyForce, massc); - //Linear drag - Vec2 dragForce = body.GetLinearVelocityFromWorldPoint(areac) - Velocity; - dragForce *= -LinearDrag * area; - body.ApplyForce(dragForce, areac); - //Angular drag - //TODO: Something that makes more physical sense? - body.ApplyTorque(-body.GetInertia() / body.GetMass() * area * body.GetAngularVelocity() * AngularDrag); - - } - } - - public override void Draw(DebugDraw debugDraw) - { - float r = 1000; - Vec2 p1 = Offset * Normal + Vec2.Cross(Normal, r); - Vec2 p2 = Offset * Normal - Vec2.Cross(Normal, r); - - Color color = new Color(0, 0, 0.8f); - - debugDraw.DrawSegment(p1, p2, color); - } - } -} \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs.meta deleted file mode 100644 index 7785241..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs.meta +++ /dev/null @@ -1,11 +0,0 @@ -fileFormatVersion: 2 -guid: 41543fe534c548f4b817e2ae3fab50e5 -MonoImporter: - externalObjects: {} - serializedVersion: 2 - defaultReferences: [] - executionOrder: 0 - icon: {instanceID: 0} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs deleted file mode 100644 index 421ca04..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs +++ /dev/null @@ -1,56 +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. -*/ - -using Box2DX.Common; - -namespace Box2DX.Dynamics.Controllers -{ - - /// This class is used to build constant acceleration controllers - public class ConstantAccelControllerDef - { - /// - /// The force to apply - /// - public Vec2 A; - } - - public class ConstantAccelController : Controller - { - /// - /// The force to apply - /// - public Vec2 A; - - public ConstantAccelController(ConstantAccelControllerDef def) - { - A = def.A; - } - - public override void Step(TimeStep step) - { - for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) - { - Body body = i.body; - if (body.IsSleeping()) - continue; - body.SetLinearVelocity(body.GetLinearVelocity() + step.Dt * A); - } - } - } -} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs.meta deleted file mode 100644 index 80e33fc..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs.meta +++ /dev/null @@ -1,11 +0,0 @@ -fileFormatVersion: 2 -guid: 9c5b0816bc173a74f8f8534af62524d4 -MonoImporter: - externalObjects: {} - serializedVersion: 2 - defaultReferences: [] - executionOrder: 0 - icon: {instanceID: 0} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs deleted file mode 100644 index 8ef9a57..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs +++ /dev/null @@ -1,57 +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. -*/ - -using Box2DX.Common; - -namespace Box2DX.Dynamics.Controllers -{ - - /// - /// This class is used to build constant force controllers - /// - public class ConstantForceControllerDef - { - /// The force to apply - public Vec2 F; - } - - public class ConstantForceController : Controller - { - /// - /// The force to apply - /// - Vec2 F; - - public ConstantForceController(ConstantForceControllerDef def) - { - F = def.F; - } - - public override void Step(TimeStep step) - { - //B2_NOT_USED(step); - for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) - { - Body body = i.body; - if (body.IsSleeping()) - continue; - body.ApplyForce(F, body.GetWorldCenter()); - } - } - } -} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs.meta deleted file mode 100644 index fb47c5a..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs.meta +++ /dev/null @@ -1,11 +0,0 @@ -fileFormatVersion: 2 -guid: aa5d8d9cf6eba0b46bd1484725fb9423 -MonoImporter: - externalObjects: {} - serializedVersion: 2 - defaultReferences: [] - executionOrder: 0 - icon: {instanceID: 0} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs deleted file mode 100644 index 26bfbdf..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs +++ /dev/null @@ -1,174 +0,0 @@ -using System; - -namespace Box2DX.Dynamics.Controllers -{ - /// - /// A controller edge is used to connect bodies and controllers together - /// in a bipartite graph. - /// - public class ControllerEdge - { - public Controller controller; // provides quick access to other end of this edge. - public Body body; // the body - public ControllerEdge prevBody; // the previous controller edge in the controllers's joint list - public ControllerEdge nextBody; // the next controller edge in the controllers's joint list - public ControllerEdge prevController; // the previous controller edge in the body's joint list - public ControllerEdge nextController; // the next controller edge in the body's joint list - } - - /// - /// Base class for controllers. Controllers are a convience for encapsulating common - /// per-step functionality. - /// - public abstract class Controller : IDisposable - { - internal Controller _prev; - internal Controller _next; - - internal World _world; - protected ControllerEdge _bodyList; - protected int _bodyCount; - - public Controller() - { - _bodyList = null; - _bodyCount = 0; - } - - public Controller(World world) - { - _bodyList = null; - _bodyCount = 0; - - _world = world; - } - - public void Dispose() - { - //Remove attached bodies - - //Previus implementation: - //while (_bodyCount > 0) - // RemoveBody(_bodyList.body); - - Clear(); - } - - /// - /// Controllers override this to implement per-step functionality. - /// - public abstract void Step(TimeStep step); - - /// - /// Controllers override this to provide debug drawing. - /// - public virtual void Draw(DebugDraw debugDraw) { } - - /// - /// Adds a body to the controller list. - /// - public void AddBody(Body body) - { - ControllerEdge edge = new ControllerEdge(); - - edge.body = body; - edge.controller = this; - - //Add edge to controller list - edge.nextBody = _bodyList; - edge.prevBody = null; - if (_bodyList != null) - _bodyList.prevBody = edge; - _bodyList = edge; - ++_bodyCount; - - //Add edge to body list - edge.nextController = body._controllerList; - edge.prevController = null; - if (body._controllerList != null) - body._controllerList.prevController = edge; - body._controllerList = edge; - } - - /// - /// Removes a body from the controller list. - /// - public void RemoveBody(Body body) - { - //Assert that the controller is not empty - Box2DXDebug.Assert(_bodyCount > 0); - - //Find the corresponding edge - ControllerEdge edge = _bodyList; - while (edge != null && edge.body != body) - edge = edge.nextBody; - - //Assert that we are removing a body that is currently attached to the controller - Box2DXDebug.Assert(edge != null); - - //Remove edge from controller list - if (edge.prevBody != null) - edge.prevBody.nextBody = edge.nextBody; - if (edge.nextBody != null) - edge.nextBody.prevBody = edge.prevBody; - if (edge == _bodyList) - _bodyList = edge.nextBody; - --_bodyCount; - - //Remove edge from body list - if (edge.prevController != null) - edge.prevController.nextController = edge.nextController; - if (edge.nextController != null) - edge.nextController.prevController = edge.prevController; - if (edge == body._controllerList) - body._controllerList = edge.nextController; - - //Free the edge - edge = null; - } - - /// - /// Removes all bodies from the controller list. - /// - public void Clear() - { -#warning "Check this" - ControllerEdge current = _bodyList; - while (current != null) - { - ControllerEdge edge = current; - - //Remove edge from controller list - _bodyList = edge.nextBody; - - //Remove edge from body list - if (edge.prevController != null) - edge.prevController.nextController = edge.nextController; - if (edge.nextController != null) - edge.nextController.prevController = edge.prevController; - if (edge == edge.body._controllerList) - edge.body._controllerList = edge.nextController; - - //Free the edge - //m_world->m_blockAllocator.Free(edge, sizeof(b2ControllerEdge)); - } - - _bodyCount = 0; - } - - /// - /// Get the next body in the world's body list. - /// - internal Controller GetNext() { return _next; } - - /// - /// Get the parent world of this body. - /// - internal World GetWorld() { return _world; } - - /// - /// Get the attached body list - /// - internal ControllerEdge GetBodyList() { return _bodyList; } - } -} \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs.meta deleted file mode 100644 index 197cb7f..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs.meta +++ /dev/null @@ -1,11 +0,0 @@ -fileFormatVersion: 2 -guid: 5cca52c5b9850aa469cd4033061b8049 -MonoImporter: - externalObjects: {} - serializedVersion: 2 - defaultReferences: [] - executionOrder: 0 - icon: {instanceID: 0} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs deleted file mode 100644 index 333e51c..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs +++ /dev/null @@ -1,96 +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. -*/ - -using Box2DX.Common; -using Math = Box2DX.Common.Math; - -namespace Box2DX.Dynamics.Controllers -{ - /// This class is used to build gravity controllers - public class GravityControllerDef - { - /// - /// Specifies the strength of the gravitiation force - /// - public float G; - - /// - /// If true, gravity is proportional to r^-2, otherwise r^-1 - /// - public bool InvSqr; - } - - public class GravityController : Controller - { - /// - /// Specifies the strength of the gravitiation force - /// - public float G; - - /// If true, gravity is proportional to r^-2, otherwise r^-1 - public bool InvSqr; - - public GravityController(GravityControllerDef def) - { - G = def.G; - InvSqr = def.InvSqr; - } - - public override void Step(TimeStep step) - { - //B2_NOT_USED(step); - if (InvSqr) - { - for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) - { - Body body1 = i.body; - for (ControllerEdge j = _bodyList; j != i; j = j.nextBody) - { - Body body2 = j.body; - Vec2 d = body2.GetWorldCenter() - body1.GetWorldCenter(); - float r2 = d.LengthSquared(); - if (r2 < Settings.FLT_EPSILON) - continue; - - Vec2 f = G / r2 / Math.Sqrt(r2) * body1.GetMass() * body2.GetMass() * d; - body1.ApplyForce(f, body1.GetWorldCenter()); - body2.ApplyForce(-1.0f * f, body2.GetWorldCenter()); - } - } - } - else - { - for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) - { - Body body1 = i.body; - for (ControllerEdge j = _bodyList; j != i; j = j.nextBody) - { - Body body2 = j.body; - Vec2 d = body2.GetWorldCenter() - body1.GetWorldCenter(); - float r2 = d.LengthSquared(); - if (r2 < Settings.FLT_EPSILON) - continue; - Vec2 f = G / r2 * body1.GetMass() * body2.GetMass() * d; - body1.ApplyForce(f, body1.GetWorldCenter()); - body2.ApplyForce(-1.0f * f, body2.GetWorldCenter()); - } - } - } - } - } -} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs.meta deleted file mode 100644 index b010f74..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs.meta +++ /dev/null @@ -1,11 +0,0 @@ -fileFormatVersion: 2 -guid: d18e2e93089bd984d9a454597208b26e -MonoImporter: - externalObjects: {} - serializedVersion: 2 - defaultReferences: [] - executionOrder: 0 - icon: {instanceID: 0} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs deleted file mode 100644 index 8fb5af3..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs +++ /dev/null @@ -1,89 +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. -*/ - -using Box2DX.Common; - -namespace Box2DX.Dynamics.Controllers -{ - - /// - /// This class is used to build tensor damping controllers - /// - public class b2TensorDampingControllerDef - { - /// Tensor to use in damping model - Mat22 T; - /// Set this to a positive number to clamp the maximum amount of damping done. - float maxTimestep; - }; - - public class TensorDampingController : Controller - { - - /// - /// Tensor to use in damping model - /// Some examples (matrixes in format (row1; row2) ) - ///(-a 0;0 -a) Standard isotropic damping with strength a - ///(0 a;-a 0) Electron in fixed field - a force at right angles to velocity with proportional magnitude - ///(-a 0;0 -b) Differing x and y damping. Useful e.g. for top-down wheels. - ///By the way, tensor in this case just means matrix, don't let the terminology get you down. - /// - Mat22 T; - - /// - /// Set this to a positive number to clamp the maximum amount of damping done. - /// Typically one wants maxTimestep to be 1/(max eigenvalue of T), so that damping will never cause something to reverse direction - /// - float MaxTimestep; - - /// Sets damping independantly along the x and y axes - public void SetAxisAligned(float xDamping, float yDamping) - { - T.Col1.X = -xDamping; - T.Col1.Y = 0; - T.Col2.X = 0; - T.Col2.Y = -yDamping; - if (xDamping > 0 || yDamping > 0) - { - MaxTimestep = 1 / Math.Max(xDamping, yDamping); - } - else - { - MaxTimestep = 0; - } - } - - public override void Step(TimeStep step) - { - float timestep = step.Dt; - if (timestep <= Settings.FLT_EPSILON) - return; - if (timestep > MaxTimestep && MaxTimestep > 0) - timestep = MaxTimestep; - for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) - { - Body body = i.body; - if (body.IsSleeping()) - continue; - - Vec2 damping = body.GetWorldVector(Math.Mul(T, body.GetLocalVector(body.GetLinearVelocity()))); - body.SetLinearVelocity(body.GetLinearVelocity() + timestep*damping); - } - } - } -} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs.meta deleted file mode 100644 index a975c8f..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs.meta +++ /dev/null @@ -1,11 +0,0 @@ -fileFormatVersion: 2 -guid: c119a4aba6eaccf408daeb709363782c -MonoImporter: - externalObjects: {} - serializedVersion: 2 - defaultReferences: [] - executionOrder: 0 - icon: {instanceID: 0} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs b/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs deleted file mode 100644 index eecfd1f..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs +++ /dev/null @@ -1,478 +0,0 @@ -/* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 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. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using Box2DX.Collision; -using Box2DX.Common; - -namespace Box2DX.Dynamics -{ - /// - /// This holds contact filtering data. - /// - public struct FilterData - { - /// - /// The collision category bits. Normally you would just set one bit. - /// - public ushort CategoryBits; - - /// - /// The collision mask bits. This states the categories that this - /// shape would accept for collision. - /// - public ushort MaskBits; - - /// - /// Collision groups allow a certain group of objects to never collide (negative) - /// or always collide (positive). Zero means no collision group. Non-zero group - /// filtering always wins against the mask bits. - /// - public short GroupIndex; - } - - /// - /// A fixture definition is used to create a fixture. This class defines an - /// abstract fixture definition. You can reuse fixture definitions safely. - /// - public class FixtureDef - { - /// - /// The constructor sets the default fixture definition values. - /// - public FixtureDef() - { - Type = ShapeType.UnknownShape; - UserData = null; - Friction = 0.2f; - Restitution = 0.0f; - Density = 0.0f; - Filter.CategoryBits = 0x0001; - Filter.MaskBits = 0xFFFF; - Filter.GroupIndex = 0; - IsSensor = false; - } - - /// - /// Holds the shape type for down-casting. - /// - public ShapeType Type; - - /// - /// Use this to store application specific fixture data. - /// - public object UserData; - - /// - /// The friction coefficient, usually in the range [0,1]. - /// - public float Friction; - - /// - /// The restitution (elasticity) usually in the range [0,1]. - /// - public float Restitution; - - /// - /// The density, usually in kg/m^2. - /// - public float Density; - - /// - /// A sensor shape collects contact information but never generates a collision response. - /// - public bool IsSensor; - - /// - /// Contact filtering data. - /// - public FilterData Filter; - } - - /// - /// This structure is used to build a fixture with a circle shape. - /// - public class CircleDef : FixtureDef - { - public Vec2 LocalPosition; - public float Radius; - - public CircleDef() - { - Type = ShapeType.CircleShape; - LocalPosition = Vec2.Zero; - Radius = 1.0f; - } - } - - /// - /// Convex polygon. The vertices must be ordered so that the outside of - /// the polygon is on the right side of the edges (looking along the edge - /// from start to end). - /// - public class PolygonDef : FixtureDef - { - /// - /// The number of polygon vertices. - /// - public int VertexCount; - - /// - /// The polygon vertices in local coordinates. - /// - public Vec2[] Vertices = new Vec2[Settings.MaxPolygonVertices]; - - public PolygonDef() - { - Type = ShapeType.PolygonShape; - VertexCount = 0; - } - - /// - /// Build vertices to represent an axis-aligned box. - /// - /// The half-width - /// The half-height. - public void SetAsBox(float hx, float hy) - { - VertexCount = 4; - Vertices[0].Set(-hx, -hy); - Vertices[1].Set(hx, -hy); - Vertices[2].Set(hx, hy); - Vertices[3].Set(-hx, hy); - } - - - /// - /// Build vertices to represent an oriented box. - /// - /// The half-width - /// The half-height. - /// The center of the box in local coordinates. - /// The rotation of the box in local coordinates. - public void SetAsBox(float hx, float hy, Vec2 center, float angle) - { - SetAsBox(hx, hy); - - XForm xf = new XForm(); - xf.Position = center; - xf.R.Set(angle); - - for (int i = 0; i < VertexCount; ++i) - { - Vertices[i] = Common.Math.Mul(xf, Vertices[i]); - } - } - } - - /// - /// This structure is used to build a chain of edges. - /// - public class EdgeDef : FixtureDef - { - public EdgeDef() - { - Type = ShapeType.EdgeShape; - } - - /// - /// The start vertex. - /// - public Vec2 Vertex1; - - /// - /// The end vertex. - /// - public Vec2 Vertex2; - } - - /// - /// 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 - /// such as friction, collision filters, etc. - /// Fixtures are created via Body.CreateFixture. - /// @warning you cannot reuse fixtures. - /// - public class Fixture - { - protected ShapeType _type; - protected bool _isSensor; - protected UInt16 _proxyId; - - internal Body _body; - protected Shape _shape; - internal Fixture _next; - - /// - /// Contact filtering data. You must call b2World::Refilter to correct - /// existing contacts/non-contacts. - /// - public FilterData Filter; - - /// - /// Is this fixture a sensor (non-solid)? - /// - public bool IsSensor { get { return _isSensor; } } - - /// - /// Get the child shape. You can modify the child shape, however you should not change the - /// number of vertices because this will crash some collision caching mechanisms. - /// - public Shape Shape { get { return _shape; } } - - /// - /// Get the type of this shape. You can use this to down cast to the concrete shape. - /// - public ShapeType ShapeType { get { return _type; } } - - /// - /// Get the next fixture in the parent body's fixture list. - /// - public Fixture Next { get { return _next; } } - - /// - /// Get the parent body of this fixture. This is NULL if the fixture is not attached. - /// - public Body Body { get { return _body; } } - - /// - /// User data that was assigned in the fixture definition. Use this to - /// store your application specific data. - /// - public object UserData; - - /// - /// Friction coefficient, usually in the range [0,1]. - /// - public float Friction; - - /// - /// Restitution (elasticity) usually in the range [0,1]. - /// - public float Restitution; - - /// - /// Density, usually in kg/m^2. - /// - public float Density; - - public Fixture() - { - _proxyId = PairManager.NullProxy; - } - - public void Create(BroadPhase broadPhase, Body body, XForm xf, FixtureDef def) - { - UserData = def.UserData; - Friction = def.Friction; - Restitution = def.Restitution; - Density = def.Density; - - _body = body; - _next = null; - - Filter = def.Filter; - - _isSensor = def.IsSensor; - - _type = def.Type; - - // Allocate and initialize the child shape. - switch (_type) - { - case ShapeType.CircleShape: - { - CircleShape circle = new CircleShape(); - CircleDef circleDef = (CircleDef)def; - circle._position = circleDef.LocalPosition; - circle._radius = circleDef.Radius; - _shape = circle; - } - break; - - case ShapeType.PolygonShape: - { - PolygonShape polygon = new PolygonShape(); - PolygonDef polygonDef = (PolygonDef)def; - polygon.Set(polygonDef.Vertices, polygonDef.VertexCount); - _shape = polygon; - } - break; - - case ShapeType.EdgeShape: - { - EdgeShape edge = new EdgeShape(); - EdgeDef edgeDef = (EdgeDef)def; - edge.Set(edgeDef.Vertex1, edgeDef.Vertex2); - _shape = edge; - } - break; - - default: - Box2DXDebug.Assert(false); - break; - } - - // Create proxy in the broad-phase. - AABB aabb; - _shape.ComputeAABB(out aabb, xf); - - bool inRange = broadPhase.InRange(aabb); - - // You are creating a shape outside the world box. - Box2DXDebug.Assert(inRange); - - if (inRange) - { - _proxyId = broadPhase.CreateProxy(aabb, this); - } - else - { - _proxyId = PairManager.NullProxy; - } - } - - public void Destroy(BroadPhase broadPhase) - { - // Remove proxy from the broad-phase. - if (_proxyId != PairManager.NullProxy) - { - broadPhase.DestroyProxy(_proxyId); - _proxyId = PairManager.NullProxy; - } - - // Free the child shape. - _shape.Dispose(); - _shape = null; - } - - internal bool Synchronize(BroadPhase broadPhase, XForm transform1, XForm transform2) - { - if (_proxyId == PairManager.NullProxy) - { - return false; - } - - // Compute an AABB that covers the swept shape (may miss some rotation effect). - AABB aabb1, aabb2; - _shape.ComputeAABB(out aabb1, transform1); - _shape.ComputeAABB(out aabb2, transform2); - - AABB aabb = new AABB(); - aabb.Combine(aabb1, aabb2); - - if (broadPhase.InRange(aabb)) - { - broadPhase.MoveProxy(_proxyId, aabb); - return true; - } - else - { - return false; - } - } - - internal void RefilterProxy(BroadPhase broadPhase, XForm transform) - { - if (_proxyId == PairManager.NullProxy) - { - return; - } - - broadPhase.DestroyProxy(_proxyId); - - AABB aabb; - _shape.ComputeAABB(out aabb, transform); - - bool inRange = broadPhase.InRange(aabb); - - if (inRange) - { - _proxyId = broadPhase.CreateProxy(aabb, this); - } - else - { - _proxyId = PairManager.NullProxy; - } - } - - public virtual void Dispose() - { - Box2DXDebug.Assert(_proxyId == PairManager.NullProxy); - Box2DXDebug.Assert(_shape == null); - } - - /// - /// Compute the mass properties of this shape using its dimensions and density. - /// The inertia tensor is computed about the local origin, not the centroid. - /// - /// Returns the mass data for this shape. - public void ComputeMass(out MassData massData) - { - _shape.ComputeMass(out massData, Density); - } - - /// - /// Compute the volume and centroid of this fixture intersected with a half plane. - /// - /// Normal the surface normal. - /// Offset the surface offset along normal. - /// Returns the centroid. - /// The total volume less than offset along normal. - public float ComputeSubmergedArea(Vec2 normal, float offset, out Vec2 c) - { - return _shape.ComputeSubmergedArea(normal, offset, _body.GetXForm(), out c); - } - - /// - /// Test a point for containment in this fixture. This only works for convex shapes. - /// - /// A point in world coordinates. - public bool TestPoint(Vec2 p) - { - return _shape.TestPoint(_body.GetXForm(), p); - } - - /// - /// Perform a ray cast against this shape. - /// - /// Returns the hit fraction. You can use this to compute the contact point - /// p = (1 - lambda) * segment.p1 + lambda * segment.p2. - /// Returns the normal at the contact point. If there is no intersection, the normal - /// is not set. - /// Defines the begin and end point of the ray cast. - /// A number typically in the range [0,1]. - public SegmentCollide TestSegment(out float lambda, out Vec2 normal, Segment segment, float maxLambda) - { - return _shape.TestSegment(_body.GetXForm(), out lambda, out normal, segment, maxLambda); - } - - /// - /// Get the maximum radius about the parent body's center of mass. - /// - public float ComputeSweepRadius(Vec2 pivot) - { - return _shape.ComputeSweepRadius(pivot); - } - } -} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs.meta deleted file mode 100644 index c5f3148..0000000 --- a/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs.meta +++ /dev/null @@ -1,11 +0,0 @@ -fileFormatVersion: 2 -guid: 121b855d9b0acd24c8362d3416dc5907 -MonoImporter: - externalObjects: {} - serializedVersion: 2 - defaultReferences: [] - executionOrder: 0 - icon: {instanceID: 0} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Island.cs b/Box2d/Assets/Program/Box2d/Dynamics/Island.cs index 0f24b8e..2fa4bd8 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Island.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Island.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -106,9 +106,9 @@ to increase the number of cache hits. Much of misses are due to random access to body data. The constraint structures are iterated over linearly, which leads to few cache misses. -The _bodies are not accessed during iteration. Instead read only data, such as +The bodies are not accessed during iteration. Instead read only data, such as the mass values are stored with the constraints. The mutable data are the constraint -impulses and the _bodies velocities/positions. The impulses are held inside the +impulses and the bodies velocities/positions. The impulses are held inside the constraint structures. The body velocities/positions are held in compact, temporary arrays to increase the number of cache hits. Linear and angular velocity are stored in a single array since multiple arrays lead to multiple misses. @@ -184,9 +184,9 @@ namespace Box2DX.Dynamics _bodyCapacity = bodyCapacity; _contactCapacity = contactCapacity; _jointCapacity = jointCapacity; - //__bodyCount = 0; - //_contactCount = 0; - //_jointCount = 0; + _bodyCount = 0; + _contactCount = 0; + _jointCount = 0; _listener = listener; @@ -242,6 +242,39 @@ namespace Box2DX.Dynamics // v2 = (1.0f - c * dt) * v1 b._linearVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f); b._angularVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f); + + // Check for large velocities. +#if TARGET_FLOAT32_IS_FIXED + // Fixed point code written this way to prevent + // overflows, float code is optimized for speed + + float vMagnitude = b._linearVelocity.Length(); + if(vMagnitude > Settings.MaxLinearVelocity) + { + b._linearVelocity *= Settings.MaxLinearVelocity/vMagnitude; + } + b._angularVelocity = Vector2.Clamp(b._angularVelocity, + -Settings.MaxAngularVelocity, Settings.MaxAngularVelocity); + +#else + if (Vec2.Dot(b._linearVelocity, b._linearVelocity) > Settings.MaxLinearVelocitySquared) + { + b._linearVelocity.Normalize(); + b._linearVelocity *= Settings.MaxLinearVelocity; + } + + if (b._angularVelocity * b._angularVelocity > Settings.MaxAngularVelocitySquared) + { + if (b._angularVelocity < 0.0f) + { + b._angularVelocity = -Settings.MaxAngularVelocity; + } + else + { + b._angularVelocity = Settings.MaxAngularVelocity; + } + } +#endif } ContactSolver contactSolver = new ContactSolver(step, _contacts, _contactCount); @@ -275,27 +308,6 @@ namespace Box2DX.Dynamics if (b.IsStatic()) continue; - // Check for large velocities. - Vec2 translation = step.Dt * b._linearVelocity; - if (Common.Vec2.Dot(translation, translation) > Settings.MaxTranslationSquared) - { - translation.Normalize(); - b._linearVelocity = (Settings.MaxTranslation * step.Inv_Dt) * translation; - } - - float rotation = step.Dt * b._angularVelocity; - if (rotation * rotation > Settings.MaxRotationSquared) - { - if (rotation < 0.0) - { - b._angularVelocity = -step.Inv_Dt * Settings.MaxRotation; - } - else - { - b._angularVelocity = step.Inv_Dt * Settings.MaxRotation; - } - } - // Store positions for continuous collision. b._sweep.C0 = b._sweep.C; b._sweep.A0 = b._sweep.A; @@ -311,14 +323,13 @@ namespace Box2DX.Dynamics } // Iterate over constraints. - for (int i = 0; i < step.PositionIterations; ++i) + for (int ii = 0; ii < step.PositionIterations; ++ii) { bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte); - bool jointsOkay = true; - for (int j = 0; j < _jointCount; ++j) + for (int i = 0; i < _jointCount; ++i) { - bool jointOkay = _joints[j].SolvePositionConstraints(Settings.ContactBaumgarte); + bool jointOkay = _joints[i].SolvePositionConstraints(/*Settings.ContactBaumgarte*/); jointsOkay = jointsOkay && jointOkay; } @@ -333,8 +344,7 @@ namespace Box2DX.Dynamics if (allowSleep) { - float minSleepTime = Settings.FLT_MAX; - + float minSleepTime = Common.Settings.FLT_MAX; #if !TARGET_FLOAT32_IS_FIXED float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance; float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance; @@ -360,7 +370,7 @@ namespace Box2DX.Dynamics Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance || Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance) #else - b._angularVelocity * b._angularVelocity > angTolSqr || + b._angularVelocity * b._angularVelocity > angTolSqr || Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr) #endif { @@ -391,24 +401,31 @@ namespace Box2DX.Dynamics { ContactSolver contactSolver = new ContactSolver(subStep, _contacts, _contactCount); - // No warm starting is needed for TOI events because warm - // starting impulses were applied in the discrete solver. + // No warm starting needed for TOI contact events. + +#if B2_TOI_JOINTS + // For joints, initialize with the last full step warm starting values + subStep.WarmStarting = true; - // Warm starting for joints is off for now, but we need to - // call this function to compute Jacobians. for (int i = 0; i < _jointCount; ++i) { _joints[i].InitVelocityConstraints(subStep); } + // ...but don't update the warm starting during TOI solve! + subStep.WarmStarting = false; +#endif + // Solve velocity constraints. for (int i = 0; i < subStep.VelocityIterations; ++i) { contactSolver.SolveVelocityConstraints(); - for (int j = 0; j < _jointCount; ++j) - { - _joints[j].SolveVelocityConstraints(subStep); - } +#if B2_TOI_JOINTS + for (int j = 0; j < _jointCount; ++j) + { + _joints[j].SolveVelocityConstraints(subStep); + } +#endif } // Don't store the TOI contact forces for warm starting @@ -422,27 +439,6 @@ namespace Box2DX.Dynamics if (b.IsStatic()) continue; - // Check for large velocities. - Vec2 translation = subStep.Dt * b._linearVelocity; - if (Vec2.Dot(translation, translation) > Settings.MaxTranslationSquared) - { - translation.Normalize(); - b._linearVelocity = (Settings.MaxTranslation * subStep.Inv_Dt) * translation; - } - - float rotation = subStep.Dt * b._angularVelocity; - if (rotation * rotation > Settings.MaxRotationSquared) - { - if (rotation < 0.0) - { - b._angularVelocity = -subStep.Inv_Dt * Settings.MaxRotation; - } - else - { - b._angularVelocity = subStep.Inv_Dt * Settings.MaxRotation; - } - } - // Store positions for continuous collision. b._sweep.C0 = b._sweep.C; b._sweep.A0 = b._sweep.A; @@ -458,21 +454,28 @@ namespace Box2DX.Dynamics } // Solve position constraints. - const float k_toiBaumgarte = 0.75f; + float k_toiBaumgarte = 0.75f; for (int i = 0; i < subStep.PositionIterations; ++i) { bool contactsOkay = contactSolver.SolvePositionConstraints(k_toiBaumgarte); +#if B2_TOI_JOINTS bool jointsOkay = true; for (int j = 0; j < _jointCount; ++j) { - bool jointOkay = _joints[j].SolvePositionConstraints(k_toiBaumgarte); + bool jointOkay = _joints[j].SolvePositionConstraints(); jointsOkay = jointsOkay && jointOkay; } - + if (contactsOkay && jointsOkay) { break; } +#else + if (contactsOkay) + { + break; + } +#endif } Report(contactSolver._constraints); @@ -508,14 +511,31 @@ namespace Box2DX.Dynamics { Contact c = _contacts[i]; ContactConstraint cc = constraints[i]; - ContactImpulse impulse = new ContactImpulse(); - for (int j = 0; j < cc.PointCount; ++j) + ContactResult cr = new ContactResult(); + cr.Shape1 = c.GetShape1(); + cr.Shape2 = c.GetShape2(); + Body b1 = cr.Shape1.GetBody(); + int manifoldCount = c.GetManifoldCount(); + Manifold[] manifolds = c.GetManifolds(); + for (int j = 0; j < manifoldCount; ++j) { - impulse.normalImpulses[j] = cc.Points[j].NormalImpulse; - impulse.tangentImpulses[j] = cc.Points[j].TangentImpulse; - } + Manifold manifold = manifolds[j]; + cr.Normal = manifold.Normal; + for (int k = 0; k < manifold.PointCount; ++k) + { + ManifoldPoint point = manifold.Points[k]; + ContactConstraintPoint ccp = cc.Points[k]; + cr.Position = b1.GetWorldPoint(point.LocalPoint1); + + // TOI constraint results are not stored, so get + // the result from the constraint. + cr.NormalImpulse = ccp.NormalImpulse; + cr.TangentImpulse = ccp.TangentImpulse; + cr.ID = point.ID; - _listener.PostSolve(c, impulse); + _listener.Result(cr); + } + } } } } diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/DistanceJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/DistanceJoint.cs index f00b664..6b104dc 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/DistanceJoint.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/DistanceJoint.cs @@ -65,6 +65,10 @@ namespace Box2DX.Dynamics /// /// Initialize the bodies, anchors, and length using the world anchors. /// + /// + /// + /// + /// public void Initialize(Body body1, Body body2, Vec2 anchor1, Vec2 anchor2) { Body1 = body1; @@ -129,14 +133,14 @@ namespace Box2DX.Dynamics get { return _body2.GetWorldPoint(_localAnchor2);} } - public override Vec2 GetReactionForce(float inv_dt) + public override Vec2 ReactionForce { - return (inv_dt * _impulse) * _u; + get { return (_inv_dt * _impulse) * _u; } } - public override float GetReactionTorque(float inv_dt) + public override float ReactionTorque { - return 0.0f; + get { return 0.0f; } } public DistanceJoint(DistanceJointDef def) @@ -150,10 +154,13 @@ namespace Box2DX.Dynamics _impulse = 0.0f; _gamma = 0.0f; _bias = 0.0f; + _inv_dt = 0.0f; } internal override void InitVelocityConstraints(TimeStep step) { + _inv_dt = step.Inv_Dt; + Body b1 = _body1; Body b2 = _body2; @@ -201,7 +208,6 @@ namespace Box2DX.Dynamics if (step.WarmStarting) { - //Scale the inpulse to support a variable timestep. _impulse *= step.DtRatio; Vec2 P = _impulse * _u; b1._linearVelocity -= b1._invMass * P; @@ -215,11 +221,10 @@ namespace Box2DX.Dynamics } } - internal override bool SolvePositionConstraints(float baumgarte) + internal override bool SolvePositionConstraints() { if (_frequencyHz > 0.0f) { - //There is no possition correction for soft distace constraint. return true; } diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/GearJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/GearJoint.cs index 7dbdf6a..ebacae3 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/GearJoint.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/GearJoint.cs @@ -118,25 +118,31 @@ namespace Box2DX.Dynamics public float _mass; // Impulse for accumulation/warm starting. - public float _impulse; + public float _force; public override Vec2 Anchor1 { get { return _body1.GetWorldPoint(_localAnchor1); } } public override Vec2 Anchor2 { get { return _body2.GetWorldPoint(_localAnchor2); } } - public override Vec2 GetReactionForce(float inv_dt) + public override Vec2 ReactionForce { - // TODO_ERIN not tested - Vec2 P = _impulse * _J.Linear2; - return inv_dt * P; + get + { + // TODO_ERIN not tested + Vec2 F = Settings.FORCE_SCALE(_force) * _J.Linear2; + return F; + } } - public override float GetReactionTorque(float inv_dt) + public override float ReactionTorque { - // TODO_ERIN not tested - Vec2 r = Common.Math.Mul(_body2.GetXForm().R, _localAnchor2 - _body2.GetLocalCenter()); - Vec2 P = _impulse * _J.Linear2; - float L = _impulse * _J.Angular2 - Vec2.Cross(r, P); - return inv_dt * L; + get + { + // TODO_ERIN not tested + Vec2 r = Common.Math.Mul(_body2.GetXForm().R, _localAnchor2 - _body2.GetLocalCenter()); + Vec2 F = _force * _J.Linear2; + float T = Settings.FORCE_SCALE(_force * _J.Angular2 - Vec2.Cross(r, F)); + return T; + } } /// @@ -200,7 +206,7 @@ namespace Box2DX.Dynamics _constant = coordinate1 + _ratio * coordinate2; - _impulse = 0.0f; + _force = 0.0f; } internal override void InitVelocityConstraints(TimeStep step) @@ -250,14 +256,15 @@ namespace Box2DX.Dynamics if (step.WarmStarting) { // Warm starting. - b1._linearVelocity += b1._invMass * _impulse * _J.Linear1; - b1._angularVelocity += b1._invI * _impulse * _J.Angular1; - b2._linearVelocity += b2._invMass * _impulse * _J.Linear2; - b2._angularVelocity += b2._invI * _impulse * _J.Angular2; + float P = Settings.FORCE_SCALE(step.Dt) * _force; + b1._linearVelocity += b1._invMass * P * _J.Linear1; + b1._angularVelocity += b1._invI * P * _J.Angular1; + b2._linearVelocity += b2._invMass * P * _J.Linear2; + b2._angularVelocity += b2._invI * P * _J.Angular2; } else { - _impulse = 0.0f; + _force = 0.0f; } } @@ -266,18 +273,20 @@ namespace Box2DX.Dynamics Body b1 = _body1; Body b2 = _body2; - float Cdot = _J.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity); + float Cdot = _J.Compute(b1._linearVelocity, b1._angularVelocity, + b2._linearVelocity, b2._angularVelocity); - float impulse = _mass * (-Cdot); - _impulse += impulse; + float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _mass * Cdot; + _force += force; - b1._linearVelocity += b1._invMass * impulse * _J.Linear1; - b1._angularVelocity += b1._invI * impulse * _J.Angular1; - b2._linearVelocity += b2._invMass * impulse * _J.Linear2; - b2._angularVelocity += b2._invI * impulse * _J.Angular2; + float P = Settings.FORCE_SCALE(step.Dt) * force; + b1._linearVelocity += b1._invMass * P * _J.Linear1; + b1._angularVelocity += b1._invI * P * _J.Angular1; + b2._linearVelocity += b2._invMass * P * _J.Linear2; + b2._angularVelocity += b2._invI * P * _J.Angular2; } - internal override bool SolvePositionConstraints(float baumgarte) + internal override bool SolvePositionConstraints() { float linearError = 0.0f; @@ -305,7 +314,7 @@ namespace Box2DX.Dynamics float C = _constant - (coordinate1 + _ratio * coordinate2); - float impulse = _mass * (-C); + float impulse = -_mass * C; b1._sweep.C += b1._invMass * impulse * _J.Linear1; b1._sweep.A += b1._invI * impulse * _J.Angular1; @@ -315,8 +324,7 @@ namespace Box2DX.Dynamics b1.SynchronizeTransform(); b2.SynchronizeTransform(); - //TODO_ERIN not implemented return linearError < Settings.LinearSlop; } } -} +} \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/Joint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/Joint.cs index 232f98b..f3265b4 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/Joint.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/Joint.cs @@ -35,8 +35,7 @@ namespace Box2DX.Dynamics DistanceJoint, PulleyJoint, MouseJoint, - GearJoint, - LineJoint + GearJoint } public enum LimitState @@ -158,19 +157,17 @@ namespace Box2DX.Dynamics internal Body _body1; internal Body _body2; + protected float _inv_dt; + internal bool _islandFlag; internal bool _collideConnected; protected object _userData; - // Cache here per time step to reduce cache misses. - protected Vec2 _localCenter1, _localCenter2; - protected float _invMass1, _invI1; - protected float _invMass2, _invI2; - /// /// Get the type of the concrete joint. /// + /// public new JointType GetType() { return _type; @@ -208,13 +205,15 @@ namespace Box2DX.Dynamics /// /// Get the reaction force on body2 at the joint anchor. - /// - public abstract Vec2 GetReactionForce(float inv_dt); + /// + /// + public abstract Vec2 ReactionForce { get; } /// /// Get the reaction torque on body2. - /// - public abstract float GetReactionTorque(float inv_dt); + /// + /// + public abstract float ReactionTorque { get; } /// /// Get the next joint the world joint list. @@ -258,36 +257,37 @@ namespace Box2DX.Dynamics joint = new DistanceJoint((DistanceJointDef)def); } break; + case JointType.MouseJoint: { joint = new MouseJoint((MouseJointDef)def); } break; + case JointType.PrismaticJoint: { joint = new PrismaticJoint((PrismaticJointDef)def); } break; + case JointType.RevoluteJoint: { joint = new RevoluteJoint((RevoluteJointDef)def); } break; + case JointType.PulleyJoint: { joint = new PulleyJoint((PulleyJointDef)def); } break; + case JointType.GearJoint: { joint = new GearJoint((GearJointDef)def); } break; - case JointType.LineJoint: - { - joint = new LineJoint((LineJointDef)def); - } - break; + default: Box2DXDebug.Assert(false); break; @@ -305,12 +305,7 @@ namespace Box2DX.Dynamics internal abstract void SolveVelocityConstraints(TimeStep step); // This returns true if the position errors are within tolerance. - internal abstract bool SolvePositionConstraints(float baumgarte); - - internal void ComputeXForm(ref XForm xf, Vec2 center, Vec2 localCenter, float angle) - { - xf.R.Set(angle); - xf.Position = center - Box2DX.Common.Math.Mul(xf.R, localCenter); - } + internal virtual void InitPositionConstraints() { } + internal abstract bool SolvePositionConstraints(); } } diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/LineJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/LineJoint.cs index 04b2103..5f28270 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/LineJoint.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/LineJoint.cs @@ -1,710 +1 @@ -/* - Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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. -*/ - -// 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 - -using System; -using System.Collections.Generic; -using System.Text; - -using Box2DX.Common; - -namespace Box2DX.Dynamics -{ - /// - /// 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. - /// - public class LineJointDef : JointDef - { - public LineJointDef() - { - Type = JointType.LineJoint; - localAnchor1.SetZero(); - localAnchor2.SetZero(); - localAxis1.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. - /// - public void Initialize(Body body1, Body body2, Vec2 anchor, Vec2 axis) - { - Body1 = body1; - Body2 = body2; - localAnchor1 = body1.GetLocalPoint(anchor); - localAnchor2 = body2.GetLocalPoint(anchor); - localAxis1 = body1.GetLocalVector(axis); - } - - /// - /// The local anchor point relative to body1's origin. - /// - public Vec2 localAnchor1; - - /// - /// The local anchor point relative to body2's origin. - /// - public Vec2 localAnchor2; - - /// - /// The local translation axis in body1. - /// - public Vec2 localAxis1; - - /// - /// Enable/disable the joint limit. - /// - public bool enableLimit; - - /// - /// The lower translation limit, usually in meters. - /// - public float lowerTranslation; - - /// - /// The upper translation limit, usually in meters. - /// - public float upperTranslation; - - /// - /// Enable/disable the joint motor. - /// - public bool enableMotor; - - /// - /// The maximum motor torque, usually in N-m. - /// - public float maxMotorForce; - - /// - /// The desired motor speed in radians per second. - /// - public float motorSpeed; - } - - /// - /// A line joint. This joint provides one degree of freedom: translation - /// along an axis fixed in body1. 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. - /// - public class LineJoint : Joint - { - public Vec2 _localAnchor1; - public Vec2 _localAnchor2; - public Vec2 _localXAxis1; - public Vec2 _localYAxis1; - - public Vec2 _axis, _perp; - public float _s1, _s2; - public float _a1, _a2; - - public Mat22 _K; - public Vec2 _impulse; - - public float _motorMass; // effective mass for motor/limit translational constraint. - public float _motorImpulse; - - public float _lowerTranslation; - public float _upperTranslation; - public float _maxMotorForce; - public float _motorSpeed; - - public bool _enableLimit; - public bool _enableMotor; - public LimitState _limitState; - - public LineJoint(LineJointDef def) - : base(def) - { - _localAnchor1 = def.localAnchor1; - _localAnchor2 = def.localAnchor2; - _localXAxis1 = def.localAxis1; - _localYAxis1 = Vec2.Cross(1.0f, _localXAxis1); - - _impulse.SetZero(); - _motorMass = 0.0f; - _motorImpulse = 0.0f; - - _lowerTranslation = def.lowerTranslation; - _upperTranslation = def.upperTranslation; - _maxMotorForce = Settings.FORCE_INV_SCALE(def.maxMotorForce); - _motorSpeed = def.motorSpeed; - _enableLimit = def.enableLimit; - _enableMotor = def.enableMotor; - _limitState = LimitState.InactiveLimit; - - _axis.SetZero(); - _perp.SetZero(); - } - - public override Vec2 Anchor1 - { - get { return _body1.GetWorldPoint(_localAnchor1); } - } - - public override Vec2 Anchor2 - { - get { return _body2.GetWorldPoint(_localAnchor2); } - } - - public override Vec2 GetReactionForce(float inv_dt) - { - return inv_dt * (_impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis); - } - - public override float GetReactionTorque(float inv_dt) - { - return 0.0f; - } - - /// - /// Get the current joint translation, usually in meters. - /// - public float GetJointTranslation() - { - Body b1 = _body1; - Body b2 = _body2; - - Vec2 p1 = b1.GetWorldPoint(_localAnchor1); - Vec2 p2 = b2.GetWorldPoint(_localAnchor2); - Vec2 d = p2 - p1; - Vec2 axis = b1.GetWorldVector(_localXAxis1); - - float translation = Vec2.Dot(d, axis); - return translation; - } - - /// - /// Get the current joint translation speed, usually in meters per second. - /// - public float GetJointSpeed() - { - Body b1 = _body1; - Body b2 = _body2; - - Vec2 r1 = Box2DX.Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); - Vec2 r2 = Box2DX.Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); - Vec2 p1 = b1._sweep.C + r1; - Vec2 p2 = b2._sweep.C + r2; - Vec2 d = p2 - p1; - Vec2 axis = b1.GetWorldVector(_localXAxis1); - - Vec2 v1 = b1._linearVelocity; - Vec2 v2 = b2._linearVelocity; - float w1 = b1._angularVelocity; - float w2 = b2._angularVelocity; - - float speed = Vec2.Dot(d, Vec2.Cross(w1, axis)) + Vec2.Dot(axis, v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1)); - return speed; - } - - /// - /// Is the joint limit enabled? - /// - public bool IsLimitEnabled() - { - return _enableLimit; - } - - /// - /// Enable/disable the joint limit. - /// - public void EnableLimit(bool flag) - { - _body1.WakeUp(); - _body2.WakeUp(); - _enableLimit = flag; - } - - /// - /// Get the lower joint limit, usually in meters. - /// - public float GetLowerLimit() - { - return _lowerTranslation; - } - - /// - /// Get the upper joint limit, usually in meters. - /// - public float GetUpperLimit() - { - return _upperTranslation; - } - - /// - /// Set the joint limits, usually in meters. - /// - public void SetLimits(float lower, float upper) - { - Box2DXDebug.Assert(lower <= upper); - _body1.WakeUp(); - _body2.WakeUp(); - _lowerTranslation = lower; - _upperTranslation = upper; - } - - /// - /// Is the joint motor enabled? - /// - public bool IsMotorEnabled() - { - return _enableMotor; - } - - /// - /// Enable/disable the joint motor. - /// - public void EnableMotor(bool flag) - { - _body1.WakeUp(); - _body2.WakeUp(); - _enableMotor = flag; - } - - /// - /// Set the motor speed, usually in meters per second. - /// - public void SetMotorSpeed(float speed) - { - _body1.WakeUp(); - _body2.WakeUp(); - _motorSpeed = speed; - } - - /// - /// Set the maximum motor force, usually in N. - /// - public void SetMaxMotorForce(float force) - { - _body1.WakeUp(); - _body2.WakeUp(); - _maxMotorForce = Settings.FORCE_SCALE(1.0f) * force; - } - - /// - /// Get the current motor force, usually in N. - /// - public float GetMotorForce() - { - return _motorImpulse; - } - - /// - /// Get the motor speed, usually in meters per second. - /// - public float GetMotorSpeed() - { - return _motorSpeed; - } - - internal override void InitVelocityConstraints(TimeStep step) - { - Body b1 = _body1; - Body b2 = _body2; - - _localCenter1 = b1.GetLocalCenter(); - _localCenter2 = b2.GetLocalCenter(); - - XForm xf1 = b1.GetXForm(); - XForm xf2 = b2.GetXForm(); - - // Compute the effective masses. - Vec2 r1 = Box2DX.Common.Math.Mul(xf1.R, _localAnchor1 - _localCenter1); - Vec2 r2 = Box2DX.Common.Math.Mul(xf2.R, _localAnchor2 - _localCenter2); - Vec2 d = b2._sweep.C + r2 - b1._sweep.C - r1; - - _invMass1 = b1._invMass; - _invI1 = b1._invI; - _invMass2 = b2._invMass; - _invI2 = b2._invI; - - // Compute motor Jacobian and effective mass. - { - _axis = Box2DX.Common.Math.Mul(xf1.R, _localXAxis1); - _a1 = Vec2.Cross(d + r1, _axis); - _a2 = Vec2.Cross(r2, _axis); - - _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2; - Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON); - _motorMass = 1.0f / _motorMass; - } - - // Prismatic constraint. - { - _perp = Box2DX.Common.Math.Mul(xf1.R, _localYAxis1); - - _s1 = Vec2.Cross(d + r1, _perp); - _s2 = Vec2.Cross(r2, _perp); - - float m1 = _invMass1, m2 = _invMass2; - float i1 = _invI1, i2 = _invI2; - - float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; - float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2; - float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; - - _K.Col1.Set(k11, k12); - _K.Col2.Set(k12, k22); - } - - // Compute motor and limit terms. - if (_enableLimit) - { - float jointTranslation = Vec2.Dot(_axis, d); - if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) - { - _limitState = LimitState.EqualLimits; - } - else if (jointTranslation <= _lowerTranslation) - { - if (_limitState != LimitState.AtLowerLimit) - { - _limitState = LimitState.AtLowerLimit; - _impulse.Y = 0.0f; - } - } - else if (jointTranslation >= _upperTranslation) - { - if (_limitState != LimitState.AtUpperLimit) - { - _limitState = LimitState.AtUpperLimit; - _impulse.Y = 0.0f; - } - } - else - { - _limitState = LimitState.InactiveLimit; - _impulse.Y = 0.0f; - } - } - else - { - _limitState = LimitState.InactiveLimit; - } - - if (_enableMotor == false) - { - _motorImpulse = 0.0f; - } - - if (step.WarmStarting) - { - // Account for variable time step. - _impulse *= step.DtRatio; - _motorImpulse *= step.DtRatio; - - Vec2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis; - float L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1; - float L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2; - - b1._linearVelocity -= _invMass1 * P; - b1._angularVelocity -= _invI1 * L1; - - b2._linearVelocity += _invMass2 * P; - b2._angularVelocity += _invI2 * L2; - } - else - { - _impulse.SetZero(); - _motorImpulse = 0.0f; - } - } - - internal override void SolveVelocityConstraints(TimeStep step) - { - Body b1 = _body1; - Body b2 = _body2; - - Vec2 v1 = b1._linearVelocity; - float w1 = b1._angularVelocity; - Vec2 v2 = b2._linearVelocity; - float w2 = b2._angularVelocity; - - // Solve linear motor constraint. - if (_enableMotor && _limitState != LimitState.EqualLimits) - { - float Cdot = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; - float impulse = _motorMass * (_motorSpeed - Cdot); - float oldImpulse = _motorImpulse; - float maxImpulse = step.Dt * _maxMotorForce; - _motorImpulse = Box2DX.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = _motorImpulse - oldImpulse; - - Vec2 P = impulse * _axis; - float L1 = impulse * _a1; - float L2 = impulse * _a2; - - v1 -= _invMass1 * P; - w1 -= _invI1 * L1; - - v2 += _invMass2 * P; - w2 += _invI2 * L2; - } - - float Cdot1 = Vec2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1; - - if (_enableLimit && _limitState != LimitState.InactiveLimit) - { - // Solve prismatic and limit constraint in block form. - float Cdot2 = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; - Vec2 Cdot = new Vec2(Cdot1, Cdot2); - - Vec2 f1 = _impulse; - Vec2 df = _K.Solve(-Cdot); - _impulse += df; - - if (_limitState == LimitState.AtLowerLimit) - { - _impulse.Y = Box2DX.Common.Math.Max(_impulse.Y, 0.0f); - } - else if (_limitState == LimitState.AtUpperLimit) - { - _impulse.Y = Box2DX.Common.Math.Min(_impulse.Y, 0.0f); - } - - // f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1) - float b = -Cdot1 - (_impulse.Y - f1.Y) * _K.Col2.X; - float f2r = b / _K.Col1.X + f1.X; - _impulse.X = f2r; - - df = _impulse - f1; - - Vec2 P = df.X * _perp + df.Y * _axis; - float L1 = df.X * _s1 + df.Y * _a1; - float L2 = df.X * _s2 + df.Y * _a2; - - v1 -= _invMass1 * P; - w1 -= _invI1 * L1; - - v2 += _invMass2 * P; - w2 += _invI2 * L2; - } - else - { - // Limit is inactive, just solve the prismatic constraint in block form. - float df = (-Cdot1) / _K.Col1.X; - _impulse.X += df; - - Vec2 P = df * _perp; - float L1 = df * _s1; - float L2 = df * _s2; - - v1 -= _invMass1 * P; - w1 -= _invI1 * L1; - - v2 += _invMass2 * P; - w2 += _invI2 * L2; - } - - b1._linearVelocity = v1; - b1._angularVelocity = w1; - b2._linearVelocity = v2; - b2._angularVelocity = w2; - } - - internal override bool SolvePositionConstraints(float baumgarte) - { - Body b1 = _body1; - Body b2 = _body2; - - Vec2 c1 = b1._sweep.C; - float a1 = b1._sweep.A; - - Vec2 c2 = b2._sweep.C; - float a2 = b2._sweep.A; - - // Solve linear limit constraint. - float linearError = 0.0f, angularError = 0.0f; - bool active = false; - float C2 = 0.0f; - - Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2); - - Vec2 r1 = Box2DX.Common.Math.Mul(R1, _localAnchor1 - _localCenter1); - Vec2 r2 = Box2DX.Common.Math.Mul(R2, _localAnchor2 - _localCenter2); - Vec2 d = c2 + r2 - c1 - r1; - - if (_enableLimit) - { - _axis = Box2DX.Common.Math.Mul(R1, _localXAxis1); - - _a1 = Vec2.Cross(d + r1, _axis); - _a2 = Vec2.Cross(r2, _axis); - - float translation = Vec2.Dot(_axis, d); - if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) - { - // Prevent large angular corrections - C2 = Box2DX.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); - linearError = Box2DX.Common.Math.Abs(translation); - active = true; - } - else if (translation <= _lowerTranslation) - { - // Prevent large linear corrections and allow some slop. - C2 = Box2DX.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); - linearError = _lowerTranslation - translation; - active = true; - } - else if (translation >= _upperTranslation) - { - // Prevent large linear corrections and allow some slop. - C2 = Box2DX.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); - linearError = translation - _upperTranslation; - active = true; - } - } - - _perp = Box2DX.Common.Math.Mul(R1, _localYAxis1); - - _s1 = Vec2.Cross(d + r1, _perp); - _s2 = Vec2.Cross(r2, _perp); - - Vec2 impulse; - float C1; - C1 = Vec2.Dot(_perp, d); - - linearError = Box2DX.Common.Math.Max(linearError, Box2DX.Common.Math.Abs(C1)); - angularError = 0.0f; - - if (active) - { - float m1 = _invMass1, m2 = _invMass2; - float i1 = _invI1, i2 = _invI2; - - float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; - float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2; - float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; - - _K.Col1.Set(k11, k12); - _K.Col2.Set(k12, k22); - - Vec2 C = new Vec2(); - C.X = C1; - C.Y = C2; - - impulse = _K.Solve(-C); - } - else - { - float m1 = _invMass1, m2 = _invMass2; - float i1 = _invI1, i2 = _invI2; - - float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; - - float impulse1 = (-C1) / k11; - impulse.X = impulse1; - impulse.Y = 0.0f; - } - - Vec2 P = impulse.X * _perp + impulse.Y * _axis; - float L1 = impulse.X * _s1 + impulse.Y * _a1; - float L2 = impulse.X * _s2 + impulse.Y * _a2; - - c1 -= _invMass1 * P; - a1 -= _invI1 * L1; - c2 += _invMass2 * P; - a2 += _invI2 * L2; - - // TODO_ERIN remove need for this. - b1._sweep.C = c1; - b1._sweep.A = a1; - b2._sweep.C = c2; - b2._sweep.A = a2; - b1.SynchronizeTransform(); - b2.SynchronizeTransform(); - - return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; - } - } -} \ No newline at end of file + \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/MouseJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/MouseJoint.cs index 19eb83c..57a89a7 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/MouseJoint.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/MouseJoint.cs @@ -48,6 +48,7 @@ namespace Box2DX.Dynamics MaxForce = 0.0f; FrequencyHz = 5.0f; DampingRatio = 0.7f; + TimeStep = 1.0f / 60.0f; } /// @@ -72,6 +73,11 @@ namespace Box2DX.Dynamics /// The damping ratio. 0 = no damping, 1 = critical damping. /// public float DampingRatio; + + /// + /// The time step used in the simulation. + /// + public float TimeStep; } /// @@ -89,10 +95,8 @@ namespace Box2DX.Dynamics public Mat22 _mass; // effective mass for point-to-point constraint. public Vec2 _C; // position error public float _maxForce; - public float _frequencyHz; - public float _dampingRatio; - public float _beta; - public float _gamma; + public float _beta; // bias factor + public float _gamma; // softness public override Vec2 Anchor1 { @@ -104,19 +108,20 @@ namespace Box2DX.Dynamics get { return _body2.GetWorldPoint(_localAnchor); } } - public override Vec2 GetReactionForce(float inv_dt) + public override Vec2 ReactionForce { - return inv_dt * _impulse; + get { return Settings.FORCE_SCALE(1.0f) * _impulse; } } - public override float GetReactionTorque(float inv_dt) + public override float ReactionTorque { - return inv_dt * 0.0f; + get { return 0.0f; } } /// /// Use this to update the target point. /// + /// public void SetTarget(Vec2 target) { if (_body2.IsSleeping()) @@ -132,37 +137,29 @@ namespace Box2DX.Dynamics _target = def.Target; _localAnchor = Common.Math.MulT(_body2.GetXForm(), _target); - _maxForce = def.MaxForce; + _maxForce = Settings.FORCE_INV_SCALE(def.MaxForce); _impulse.SetZero(); - _frequencyHz = def.FrequencyHz; - _dampingRatio = def.DampingRatio; - - _beta = 0.0f; - _gamma = 0.0f; - } - - internal override void InitVelocityConstraints(TimeStep step) - { - Body b = _body2; - - float mass = b.GetMass(); + float mass = _body2._mass; // Frequency - float omega = 2.0f * Settings.Pi * _frequencyHz; + float omega = 2.0f * Settings.Pi * def.FrequencyHz; // Damping coefficient - float d = 2.0f * mass * _dampingRatio * omega; + float d = 2.0f * mass * def.DampingRatio * omega; // Spring stiffness - float k = mass * (omega * omega); + float k = (def.TimeStep * mass) * (omega * omega); // magic formulas - // gamma has units of inverse mass. - // beta has units of inverse time. - Box2DXDebug.Assert(d + step.Dt * k > Settings.FLT_EPSILON); - _gamma = 1.0f / (step.Dt * (d + step.Dt * k)); - _beta = step.Dt * k * _gamma; + Box2DXDebug.Assert(d + k > Settings.FLT_EPSILON); + _gamma = 1.0f / (d + k); + _beta = k / (d + k); + } + + internal override void InitVelocityConstraints(TimeStep step) + { + Body b = _body2; // Compute the effective mass matrix. Vec2 r = Common.Math.Mul(b.GetXForm().R, _localAnchor - b.GetLocalCenter()); @@ -185,7 +182,7 @@ namespace Box2DX.Dynamics K.Col1.X += _gamma; K.Col2.Y += _gamma; - _mass = K.GetInverse(); + _mass = K.Invert(); _C = b._sweep.C + r - _target; @@ -193,9 +190,9 @@ namespace Box2DX.Dynamics b._angularVelocity *= 0.98f; // Warm starting. - _impulse *= step.DtRatio; - b._linearVelocity += invMass * _impulse; - b._angularVelocity += invI * Vec2.Cross(r, _impulse); + Vec2 P = Settings.FORCE_SCALE(step.Dt) * _impulse; + b._linearVelocity += invMass * P; + b._angularVelocity += invI * Vec2.Cross(r, P); } internal override void SolveVelocityConstraints(TimeStep step) @@ -206,24 +203,26 @@ namespace Box2DX.Dynamics // Cdot = v + cross(w, r) Vec2 Cdot = b._linearVelocity + Vec2.Cross(b._angularVelocity, r); - Vec2 impulse = Box2DX.Common.Math.Mul(_mass, -(Cdot + _beta * _C + _gamma * _impulse)); + Vec2 force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * Common.Math.Mul(_mass, Cdot + + (_beta * step.Inv_Dt) * _C + Settings.FORCE_SCALE(step.Dt) * (_gamma * _impulse)); - Vec2 oldImpulse = _impulse; - _impulse += impulse; - float maxImpulse = step.Dt * _maxForce; - if (_impulse.LengthSquared() > maxImpulse * maxImpulse) + Vec2 oldForce = _impulse; + _impulse += force; + float forceMagnitude = _impulse.Length(); + if (forceMagnitude > _maxForce) { - _impulse *= maxImpulse / _impulse.Length(); + _impulse *= _maxForce / forceMagnitude; } - impulse = _impulse - oldImpulse; + force = _impulse - oldForce; - b._linearVelocity += b._invMass * impulse; - b._angularVelocity += b._invI * Vec2.Cross(r, impulse); + Vec2 P = Settings.FORCE_SCALE(step.Dt) * force; + b._linearVelocity += b._invMass * P; + b._angularVelocity += b._invI * Vec2.Cross(r, P); } - internal override bool SolvePositionConstraints(float baumgarte) + internal override bool SolvePositionConstraints() { return true; } } -} +} \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/PrismaticJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/PrismaticJoint.cs index c6ee2cf..8234d31 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/PrismaticJoint.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/PrismaticJoint.cs @@ -21,71 +21,21 @@ // 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)] +// C = dot(ay1, d) +// Cdot = dot(d, cross(w1, ay1)) + dot(ay1, v2 + cross(w2, r2) - v1 - cross(w1, r1)) +// = -dot(ay1, v1) - dot(cross(d + r1, ay1), w1) + dot(ay1, v2) + dot(cross(r2, ay1), v2) +// J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)] // // Angular constraint // C = a2 - a1 + a_initial // Cdot = w2 - w1 // J = [0 0 -1 0 0 1] -// -// K = J * invM * JT -// -// J = [-a -s1 a s2] -// [0 -1 0 1] -// 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 -// [0 -1 0 1] // angular -// [-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(3) = max(f2(3), 0) -// upper: f2(3) = min(f2(3), 0) -// -// Solve for correct f2(1:2) -// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1 -// = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3) -// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2) -// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) -// -// Now compute impulse to be applied: -// df = f2 - f1 - using System; using System.Collections.Generic; using System.Text; @@ -126,6 +76,10 @@ namespace Box2DX.Dynamics /// Initialize the bodies, anchors, axis, and reference angle using the world /// anchor and world axis. /// + /// + /// + /// + /// public void Initialize(Body body1, Body body2, Vec2 anchor, Vec2 axis) { Body1 = body1; @@ -201,15 +155,18 @@ namespace Box2DX.Dynamics public Vec2 _localYAxis1; public float _refAngle; - public Vec2 _axis, _perp; - public float _s1, _s2; - public float _a1, _a2; + public Jacobian _linearJacobian; + public float _linearMass; // effective mass for point-to-line constraint. + public float _force; - public Mat33 _K; - public Vec3 _impulse; + public float _angularMass; // effective mass for angular constraint. + public float _torque; + public Jacobian _motorJacobian; public float _motorMass; // effective mass for motor/limit translational constraint. - public float _motorImpulse; + public float _motorForce; + public float _limitForce; + public float _limitPositionImpulse; public float _lowerTranslation; public float _upperTranslation; @@ -230,14 +187,20 @@ namespace Box2DX.Dynamics get { return _body2.GetWorldPoint(_localAnchor2); } } - public override Vec2 GetReactionForce(float inv_dt) + public override Vec2 ReactionForce { - return inv_dt * (_impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis); + get + { + Vec2 ax1 = Common.Math.Mul(_body1.GetXForm().R, _localXAxis1); + Vec2 ay1 = Common.Math.Mul(_body1.GetXForm().R, _localYAxis1); + + return Settings.FORCE_SCALE(1.0f) * (_limitForce * ax1 + _force * ay1); + } } - public override float GetReactionTorque(float inv_dt) + public override float ReactionTorque { - return inv_dt * _impulse.Y; + get { return Settings.FORCE_SCALE(_torque); } } /// @@ -298,10 +261,9 @@ namespace Box2DX.Dynamics /// /// Enable/disable the joint limit. /// + /// public void EnableLimit(bool flag) { - _body1.WakeUp(); - _body2.WakeUp(); _enableLimit = flag; } @@ -324,11 +286,11 @@ namespace Box2DX.Dynamics /// /// Set the joint limits, usually in meters. /// + /// + /// public void SetLimits(float lower, float upper) { Box2DXDebug.Assert(lower <= upper); - _body1.WakeUp(); - _body2.WakeUp(); _lowerTranslation = lower; _upperTranslation = upper; } @@ -344,10 +306,9 @@ namespace Box2DX.Dynamics /// /// Enable/disable the joint motor. /// + /// public void EnableMotor(bool flag) { - _body1.WakeUp(); - _body2.WakeUp(); _enableMotor = flag; } @@ -357,21 +318,15 @@ namespace Box2DX.Dynamics public float MotorSpeed { get { return _motorSpeed; } - set - { - _body1.WakeUp(); - _body2.WakeUp(); - _motorSpeed = value; - } + set { _motorSpeed = value; } } /// /// Set the maximum motor force, usually in N. /// + /// public void SetMaxMotorForce(float force) { - _body1.WakeUp(); - _body2.WakeUp(); _maxMotorForce = Settings.FORCE_SCALE(1.0f) * force; } @@ -380,7 +335,7 @@ namespace Box2DX.Dynamics /// public float MotorForce { - get { return _motorImpulse; } + get { return _motorForce; } } public PrismaticJoint(PrismaticJointDef def) @@ -392,9 +347,18 @@ namespace Box2DX.Dynamics _localYAxis1 = Vec2.Cross(1.0f, _localXAxis1); _refAngle = def.ReferenceAngle; - _impulse.SetZero(); + _linearJacobian.SetZero(); + _linearMass = 0.0f; + _force = 0.0f; + + _angularMass = 0.0f; + _torque = 0.0f; + + _motorJacobian.SetZero(); _motorMass = 0.0f; - _motorImpulse = 0.0f; + _motorForce = 0.0f; + _limitForce = 0.0f; + _limitPositionImpulse = 0.0f; _lowerTranslation = def.LowerTranslation; _upperTranslation = def.UpperTranslation; @@ -402,10 +366,6 @@ namespace Box2DX.Dynamics _motorSpeed = def.MotorSpeed; _enableLimit = def.EnableLimit; _enableMotor = def.EnableMotor; - _limitState = LimitState.InactiveLimit; - - _axis.SetZero(); - _perp.SetZero(); } internal override void InitVelocityConstraints(TimeStep step) @@ -413,344 +373,286 @@ namespace Box2DX.Dynamics Body b1 = _body1; Body b2 = _body2; - // You cannot create a prismatic joint between bodies that - // both have fixed rotation. - Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f); - - _localCenter1 = b1.GetLocalCenter(); - _localCenter2 = b2.GetLocalCenter(); - - XForm xf1 = b1.GetXForm(); - XForm xf2 = b2.GetXForm(); - // Compute the effective masses. - Vec2 r1 = Box2DX.Common.Math.Mul(xf1.R, _localAnchor1 - _localCenter1); - Vec2 r2 = Box2DX.Common.Math.Mul(xf2.R, _localAnchor2 - _localCenter2); - Vec2 d = b2._sweep.C + r2 - b1._sweep.C - r1; - - _invMass1 = b1._invMass; - _invI1 = b1._invI; - _invMass2 = b2._invMass; - _invI2 = b2._invI; - - // Compute motor Jacobian and effective mass. + Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + + float invMass1 = b1._invMass, invMass2 = b2._invMass; + float invI1 = b1._invI, invI2 = b2._invI; + + // Compute point to line constraint effective mass. + // J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)] + Vec2 ay1 = Box2DXMath.Mul(b1.GetXForm().R, _localYAxis1); + Vec2 e = b2._sweep.C + r2 - b1._sweep.C; // e = d + r1 + + _linearJacobian.Set(-ay1, -Vec2.Cross(e, ay1), ay1, Vec2.Cross(r2, ay1)); + _linearMass = invMass1 + invI1 * _linearJacobian.Angular1 * _linearJacobian.Angular1 + + invMass2 + invI2 * _linearJacobian.Angular2 * _linearJacobian.Angular2; + Box2DXDebug.Assert(_linearMass > Settings.FLT_EPSILON); + _linearMass = 1.0f / _linearMass; + + // Compute angular constraint effective mass. + _angularMass = invI1 + invI2; + if (_angularMass > Settings.FLT_EPSILON) { - _axis = Box2DX.Common.Math.Mul(xf1.R, _localXAxis1); - _a1 = Vec2.Cross(d + r1, _axis); - _a2 = Vec2.Cross(r2, _axis); - - _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2; - Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON); - _motorMass = 1.0f / _motorMass; - } - - // Prismatic constraint. - { - _perp = Box2DX.Common.Math.Mul(xf1.R, _localYAxis1); - - _s1 = Vec2.Cross(d + r1, _perp); - _s2 = Vec2.Cross(r2, _perp); - - float m1 = _invMass1, m2 = _invMass2; - float i1 = _invI1, i2 = _invI2; - - float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; - float k12 = i1 * _s1 + i2 * _s2; - float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2; - float k22 = i1 + i2; - float k23 = i1 * _a1 + i2 * _a2; - float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; - - _K.Col1.Set(k11, k12, k13); - _K.Col2.Set(k12, k22, k23); - _K.Col3.Set(k13, k23, k33); + _angularMass = 1.0f / _angularMass; } // Compute motor and limit terms. - if (_enableLimit) + if (_enableLimit || _enableMotor) { - float jointTranslation = Vec2.Dot(_axis, d); - if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) - { - _limitState = LimitState.EqualLimits; - } - else if (jointTranslation <= _lowerTranslation) + // The motor and limit share a Jacobian and effective mass. + Vec2 ax1 = Box2DXMath.Mul(b1.GetXForm().R, _localXAxis1); + _motorJacobian.Set(-ax1, -Vec2.Cross(e, ax1), ax1, Vec2.Cross(r2, ax1)); + _motorMass = invMass1 + invI1 * _motorJacobian.Angular1 * _motorJacobian.Angular1 + + invMass2 + invI2 * _motorJacobian.Angular2 * _motorJacobian.Angular2; + Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON); + _motorMass = 1.0f / _motorMass; + + if (_enableLimit) { - if (_limitState != LimitState.AtLowerLimit) + Vec2 d = e - r1; // p2 - p1 + float jointTranslation = Vec2.Dot(ax1, d); + if (Box2DXMath.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) + { + _limitState = LimitState.EqualLimits; + } + else if (jointTranslation <= _lowerTranslation) { + if (_limitState != LimitState.AtLowerLimit) + { + _limitForce = 0.0f; + } _limitState = LimitState.AtLowerLimit; - _impulse.Z = 0.0f; } - } - else if (jointTranslation >= _upperTranslation) - { - if (_limitState != LimitState.AtUpperLimit) + else if (jointTranslation >= _upperTranslation) { + if (_limitState != LimitState.AtUpperLimit) + { + _limitForce = 0.0f; + } _limitState = LimitState.AtUpperLimit; - _impulse.Z = 0.0f; } - } - else - { - _limitState = LimitState.InactiveLimit; - _impulse.Z = 0.0f; + else + { + _limitState = LimitState.InactiveLimit; + _limitForce = 0.0f; + } } } - else + + if (_enableMotor == false) { - _limitState = LimitState.InactiveLimit; + _motorForce = 0.0f; } - if (_enableMotor == false) + if (_enableLimit == false) { - _motorImpulse = 0.0f; + _limitForce = 0.0f; } if (step.WarmStarting) { - // Account for variable time step. - _impulse *= step.DtRatio; - _motorImpulse *= step.DtRatio; + Vec2 P1 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Linear1 + (_motorForce + _limitForce) * _motorJacobian.Linear1); + Vec2 P2 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Linear2 + (_motorForce + _limitForce) * _motorJacobian.Linear2); + float L1 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Angular1 - _torque + (_motorForce + _limitForce) * _motorJacobian.Angular1); + float L2 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Angular2 + _torque + (_motorForce + _limitForce) * _motorJacobian.Angular2); - Vec2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis; - float L1 = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1; - float L2 = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2; + b1._linearVelocity += invMass1 * P1; + b1._angularVelocity += invI1 * L1; - b1._linearVelocity -= _invMass1 * P; - b1._angularVelocity -= _invI1 * L1; - - b2._linearVelocity += _invMass2 * P; - b2._angularVelocity += _invI2 * L2; + b2._linearVelocity += invMass2 * P2; + b2._angularVelocity += invI2 * L2; } else { - _impulse.SetZero(); - _motorImpulse = 0.0f; + _force = 0.0f; + _torque = 0.0f; + _limitForce = 0.0f; + _motorForce = 0.0f; } + + _limitPositionImpulse = 0.0f; } internal override void SolveVelocityConstraints(TimeStep step) { Body b1 = _body1; - Body b2 = _body2; + Body b2 = _body2; + + float invMass1 = b1._invMass, invMass2 = b2._invMass; + float invI1 = b1._invI, invI2 = b2._invI; + + // Solve linear constraint. + float linearCdot = _linearJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity); + float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _linearMass * linearCdot; + _force += force; - Vec2 v1 = b1._linearVelocity; - float w1 = b1._angularVelocity; - Vec2 v2 = b2._linearVelocity; - float w2 = b2._angularVelocity; + float P = Settings.FORCE_SCALE(step.Dt) * force; + b1._linearVelocity += (invMass1 * P) * _linearJacobian.Linear1; + b1._angularVelocity += invI1 * P * _linearJacobian.Angular1; + + b2._linearVelocity += (invMass2 * P) * _linearJacobian.Linear2; + b2._angularVelocity += invI2 * P * _linearJacobian.Angular2; + + // Solve angular constraint. + float angularCdot = b2._angularVelocity - b1._angularVelocity; + float torque = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _angularMass * angularCdot; + _torque += torque; + + float L = Settings.FORCE_SCALE(step.Dt) * torque; + b1._angularVelocity -= invI1 * L; + b2._angularVelocity += invI2 * L; // Solve linear motor constraint. if (_enableMotor && _limitState != LimitState.EqualLimits) { - float Cdot = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; - float impulse = _motorMass * (_motorSpeed - Cdot); - float oldImpulse = _motorImpulse; - float maxImpulse = step.Dt * _maxMotorForce; - _motorImpulse = Box2DX.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = _motorImpulse - oldImpulse; - - Vec2 P = impulse * _axis; - float L1 = impulse * _a1; - float L2 = impulse * _a2; - - v1 -= _invMass1 * P; - w1 -= _invI1 * L1; - - v2 += _invMass2 * P; - w2 += _invI2 * L2; + float motorCdot = _motorJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity) - _motorSpeed; + float motorForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _motorMass * motorCdot; + float oldMotorForce = _motorForce; + _motorForce = Box2DXMath.Clamp(_motorForce + motorForce, -_maxMotorForce, _maxMotorForce); + motorForce = _motorForce - oldMotorForce; + + float P_ = Settings.FORCE_SCALE(step.Dt) * motorForce; + b1._linearVelocity += (invMass1 * P_) * _motorJacobian.Linear1; + b1._angularVelocity += invI1 * P_ * _motorJacobian.Angular1; + + b2._linearVelocity += (invMass2 * P_) * _motorJacobian.Linear2; + b2._angularVelocity += invI2 * P_ * _motorJacobian.Angular2; } - Vec2 Cdot1; - Cdot1.X = Vec2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1; - Cdot1.Y = w2 - w1; - + // Solve linear limit constraint. if (_enableLimit && _limitState != LimitState.InactiveLimit) { - // Solve prismatic and limit constraint in block form. - float Cdot2; - Cdot2 = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; - Vec3 Cdot = new Vec3(Cdot1.X, Cdot1.Y, Cdot2); - - Vec3 f1 = _impulse; - Vec3 df = _K.Solve33(-Cdot); - _impulse += df; + float limitCdot = _motorJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity); + float limitForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _motorMass * limitCdot; - if (_limitState ==LimitState.AtLowerLimit) + if (_limitState == LimitState.EqualLimits) { - _impulse.Z = Box2DX.Common.Math.Max(_impulse.Z, 0.0f); + _limitForce += limitForce; + } + else if (_limitState == LimitState.AtLowerLimit) + { + float oldLimitForce = _limitForce; + _limitForce = Box2DXMath.Max(_limitForce + limitForce, 0.0f); + limitForce = _limitForce - oldLimitForce; } else if (_limitState == LimitState.AtUpperLimit) { - _impulse.Z = Box2DX.Common.Math.Min(_impulse.Z, 0.0f); + float oldLimitForce = _limitForce; + _limitForce = Box2DXMath.Min(_limitForce + limitForce, 0.0f); + limitForce = _limitForce - oldLimitForce; } - // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) - Vec2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vec2(_K.Col3.X, _K.Col3.Y); - Vec2 f2r = _K.Solve22(b) + new Vec2(f1.X, f1.Y); - _impulse.X = f2r.X; - _impulse.Y = f2r.Y; - - df = _impulse - f1; - - Vec2 P = df.X * _perp + df.Z * _axis; - float L1 = df.X * _s1 + df.Y + df.Z * _a1; - float L2 = df.X * _s2 + df.Y + df.Z * _a2; + float P_ = Settings.FORCE_SCALE(step.Dt) * limitForce; - v1 -= _invMass1 * P; - w1 -= _invI1 * L1; + b1._linearVelocity += (invMass1 * P_) * _motorJacobian.Linear1; + b1._angularVelocity += invI1 * P_ * _motorJacobian.Angular1; - v2 += _invMass2 * P; - w2 += _invI2 * L2; + b2._linearVelocity += (invMass2 * P_) * _motorJacobian.Linear2; + b2._angularVelocity += invI2 * P_ * _motorJacobian.Angular2; } - else - { - // Limit is inactive, just solve the prismatic constraint in block form. - Vec2 df = _K.Solve22(-Cdot1); - _impulse.X += df.X; - _impulse.Y += df.Y; - - Vec2 P = df.X * _perp; - float L1 = df.X * _s1 + df.Y; - float L2 = df.X * _s2 + df.Y; - - v1 -= _invMass1 * P; - w1 -= _invI1 * L1; - - v2 += _invMass2 * P; - w2 += _invI2 * L2; - } - - b1._linearVelocity = v1; - b1._angularVelocity = w1; - b2._linearVelocity = v2; - b2._angularVelocity = w2; } - internal override bool SolvePositionConstraints(float baumgarte) + internal override bool SolvePositionConstraints() { Body b1 = _body1; Body b2 = _body2; - Vec2 c1 = b1._sweep.C; - float a1 = b1._sweep.A; + float invMass1 = b1._invMass, invMass2 = b2._invMass; + float invI1 = b1._invI, invI2 = b2._invI; - Vec2 c2 = b2._sweep.C; - float a2 = b2._sweep.A; + Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + Vec2 p1 = b1._sweep.C + r1; + Vec2 p2 = b2._sweep.C + r2; + Vec2 d = p2 - p1; + Vec2 ay1 = Box2DXMath.Mul(b1.GetXForm().R, _localYAxis1); - // Solve linear limit constraint. - float linearError = 0.0f, angularError = 0.0f; - bool active = false; - float C2 = 0.0f; + // Solve linear (point-to-line) constraint. + float linearC = Vec2.Dot(ay1, d); + // Prevent overly large corrections. + linearC = Box2DXMath.Clamp(linearC, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); + float linearImpulse = -_linearMass * linearC; + + b1._sweep.C += (invMass1 * linearImpulse) * _linearJacobian.Linear1; + b1._sweep.A += invI1 * linearImpulse * _linearJacobian.Angular1; + //b1->SynchronizeTransform(); // updated by angular constraint + b2._sweep.C += (invMass2 * linearImpulse) * _linearJacobian.Linear2; + b2._sweep.A += invI2 * linearImpulse * _linearJacobian.Angular2; + //b2->SynchronizeTransform(); // updated by angular constraint - Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2); + float positionError = Box2DXMath.Abs(linearC); - Vec2 r1 = Box2DX.Common.Math.Mul(R1, _localAnchor1 - _localCenter1); - Vec2 r2 = Box2DX.Common.Math.Mul(R2, _localAnchor2 - _localCenter2); - Vec2 d = c2 + r2 - c1 - r1; + // Solve angular constraint. + float angularC = b2._sweep.A - b1._sweep.A - _refAngle; + // Prevent overly large corrections. + angularC = Box2DXMath.Clamp(angularC, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); + float angularImpulse = -_angularMass * angularC; - if (_enableLimit) + b1._sweep.A -= b1._invI * angularImpulse; + b2._sweep.A += b2._invI * angularImpulse; + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + + float angularError = Box2DXMath.Abs(angularC); + + // Solve linear limit constraint. + if (_enableLimit && _limitState != LimitState.InactiveLimit) { - _axis = Box2DX.Common.Math.Mul(R1, _localXAxis1); + Vec2 r1_ = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2_ = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + Vec2 p1_ = b1._sweep.C + r1_; + Vec2 p2_ = b2._sweep.C + r2_; + Vec2 d_ = p2_ - p1_; + Vec2 ax1 = Box2DXMath.Mul(b1.GetXForm().R, _localXAxis1); - _a1 = Vec2.Cross(d + r1, _axis); - _a2 = Vec2.Cross(r2, _axis); + float translation = Vec2.Dot(ax1, d_); + float limitImpulse = 0.0f; - float translation = Vec2.Dot(_axis, d); - if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) + if (_limitState == LimitState.EqualLimits) { // Prevent large angular corrections - C2 = Box2DX.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); - linearError = Box2DX.Common.Math.Abs(translation); - active = true; + float limitC = Box2DXMath.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); + limitImpulse = -_motorMass * limitC; + positionError = Box2DXMath.Max(positionError, Box2DXMath.Abs(angularC)); } - else if (translation <= _lowerTranslation) + else if (_limitState == LimitState.AtLowerLimit) { + float limitC = translation - _lowerTranslation; + positionError = Box2DXMath.Max(positionError, -limitC); + // Prevent large linear corrections and allow some slop. - C2 = Box2DX.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); - linearError = _lowerTranslation - translation; - active = true; + limitC = Box2DXMath.Clamp(limitC + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); + limitImpulse = -_motorMass * limitC; + float oldLimitImpulse = _limitPositionImpulse; + _limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f); + limitImpulse = _limitPositionImpulse - oldLimitImpulse; } - else if (translation >= _upperTranslation) + else if (_limitState == LimitState.AtUpperLimit) { + float limitC = translation - _upperTranslation; + positionError = Box2DXMath.Max(positionError, limitC); + // Prevent large linear corrections and allow some slop. - C2 = Box2DX.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); - linearError = translation - _upperTranslation; - active = true; + limitC = Box2DXMath.Clamp(limitC - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); + limitImpulse = -_motorMass * limitC; + float oldLimitImpulse = _limitPositionImpulse; + _limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f); + limitImpulse = _limitPositionImpulse - oldLimitImpulse; } - } - - _perp = Box2DX.Common.Math.Mul(R1, _localYAxis1); - - _s1 = Vec2.Cross(d + r1, _perp); - _s2 = Vec2.Cross(r2, _perp); - Vec3 impulse; - Vec2 C1 = new Vec2(); - C1.X = Vec2.Dot(_perp, d); - C1.Y = a2 - a1 - _refAngle; + b1._sweep.C += (invMass1 * limitImpulse) * _motorJacobian.Linear1; + b1._sweep.A += invI1 * limitImpulse * _motorJacobian.Angular1; + b2._sweep.C += (invMass2 * limitImpulse) * _motorJacobian.Linear2; + b2._sweep.A += invI2 * limitImpulse * _motorJacobian.Angular2; - linearError = Box2DX.Common.Math.Max(linearError, Box2DX.Common.Math.Abs(C1.X)); - angularError = Box2DX.Common.Math.Abs(C1.Y); - - if (active) - { - float m1 = _invMass1, m2 = _invMass2; - float i1 = _invI1, i2 = _invI2; - - float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; - float k12 = i1 * _s1 + i2 * _s2; - float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2; - float k22 = i1 + i2; - float k23 = i1 * _a1 + i2 * _a2; - float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; - - _K.Col1.Set(k11, k12, k13); - _K.Col2.Set(k12, k22, k23); - _K.Col3.Set(k13, k23, k33); - - Vec3 C = new Vec3(); - C.X = C1.X; - C.Y = C1.Y; - C.Z = C2; - - impulse = _K.Solve33(-C); - } - else - { - float m1 = _invMass1, m2 = _invMass2; - float i1 = _invI1, i2 = _invI2; - - float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; - float k12 = i1 * _s1 + i2 * _s2; - float k22 = i1 + i2; - - _K.Col1.Set(k11, k12, 0.0f); - _K.Col2.Set(k12, k22, 0.0f); - - Vec2 impulse1 = _K.Solve22(-C1); - impulse.X = impulse1.X; - impulse.Y = impulse1.Y; - impulse.Z = 0.0f; + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); } - Vec2 P = impulse.X * _perp + impulse.Z * _axis; - float L1 = impulse.X * _s1 + impulse.Y + impulse.Z * _a1; - float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2; - - c1 -= _invMass1 * P; - a1 -= _invI1 * L1; - c2 += _invMass2 * P; - a2 += _invI2 * L2; - - // TODO_ERIN remove need for this. - b1._sweep.C = c1; - b1._sweep.A = a1; - b2._sweep.C = c2; - b2._sweep.A = a2; - b1.SynchronizeTransform(); - b2.SynchronizeTransform(); - - return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; } } -} +} \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/PulleyJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/PulleyJoint.cs index ab92536..a7d22f9 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/PulleyJoint.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/PulleyJoint.cs @@ -88,7 +88,7 @@ namespace Box2DX.Dynamics Vec2 d2 = anchor2 - groundAnchor2; Length2 = d2.Length(); Ratio = ratio; - Box2DXDebug.Assert(ratio > Settings.FLT_EPSILON); + Box2DXDebug.Assert(ratio > Common.Settings.FLT_EPSILON); float C = Length1 + ratio * Length2; MaxLength1 = C - ratio * PulleyJoint.MinPulleyLength; MaxLength2 = (C - PulleyJoint.MinPulleyLength) / ratio; @@ -173,9 +173,14 @@ namespace Box2DX.Dynamics public float _limitMass2; // Impulses for accumulation/warm starting. - public float _impulse; - public float _limitImpulse1; - public float _limitImpulse2; + public float _force; + public float _limitForce1; + public float _limitForce2; + + // Position impulses for accumulation. + public float _positionImpulse; + public float _limitPositionImpulse1; + public float _limitPositionImpulse2; public LimitState _state; public LimitState _limitState1; @@ -191,15 +196,18 @@ namespace Box2DX.Dynamics get { return _body2.GetWorldPoint(_localAnchor2); } } - public override Vec2 GetReactionForce(float inv_dt) + public override Vec2 ReactionForce { - Vec2 P = _impulse * _u2; - return inv_dt * P; + get + { + Vec2 F = Settings.FORCE_SCALE(_force) * _u2; + return F; + } } - public override float GetReactionTorque(float inv_dt) + public override float ReactionTorque { - return 0.0f; + get { return 0.0f; } } /// @@ -271,9 +279,9 @@ namespace Box2DX.Dynamics _maxLength1 = Common.Math.Min(def.MaxLength1, _constant - _ratio * PulleyJoint.MinPulleyLength); _maxLength2 = Common.Math.Min(def.MaxLength2, (_constant - PulleyJoint.MinPulleyLength) / _ratio); - _impulse = 0.0f; - _limitImpulse1 = 0.0f; - _limitImpulse2 = 0.0f; + _force = 0.0f; + _limitForce1 = 0.0f; + _limitForce2 = 0.0f; } internal override void InitVelocityConstraints(TimeStep step) @@ -319,31 +327,34 @@ namespace Box2DX.Dynamics if (C > 0.0f) { _state = LimitState.InactiveLimit; - _impulse = 0.0f; + _force = 0.0f; } else { _state = LimitState.AtUpperLimit; + _positionImpulse = 0.0f; } if (length1 < _maxLength1) { _limitState1 = LimitState.InactiveLimit; - _limitImpulse1 = 0.0f; + _limitForce1 = 0.0f; } else { _limitState1 = LimitState.AtUpperLimit; + _limitPositionImpulse1 = 0.0f; } if (length2 < _maxLength2) { _limitState2 = LimitState.InactiveLimit; - _limitImpulse2 = 0.0f; + _limitForce2 = 0.0f; } else { _limitState2 = LimitState.AtUpperLimit; + _limitPositionImpulse2 = 0.0f; } // Compute effective mass. @@ -362,14 +373,9 @@ namespace Box2DX.Dynamics if (step.WarmStarting) { - // Scale impulses to support variable time steps. - _impulse *= step.DtRatio; - _limitImpulse1 *= step.DtRatio; - _limitImpulse2 *= step.DtRatio; - // Warm starting. - Vec2 P1 = -(_impulse + _limitImpulse1) * _u1; - Vec2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2; + Vec2 P1 = Settings.FORCE_SCALE(step.Dt) * (-_force - _limitForce1) * _u1; + Vec2 P2 = Settings.FORCE_SCALE(step.Dt) * (-_ratio * _force - _limitForce2) * _u2; b1._linearVelocity += b1._invMass * P1; b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1); b2._linearVelocity += b2._invMass * P2; @@ -377,9 +383,9 @@ namespace Box2DX.Dynamics } else { - _impulse = 0.0f; - _limitImpulse1 = 0.0f; - _limitImpulse2 = 0.0f; + _force = 0.0f; + _limitForce1 = 0.0f; + _limitForce2 = 0.0f; } } @@ -397,13 +403,13 @@ namespace Box2DX.Dynamics Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2); float Cdot = -Vec2.Dot(_u1, v1) - _ratio * Vec2.Dot(_u2, v2); - float impulse = _pulleyMass * (-Cdot); - float oldImpulse = _impulse; - _impulse = Box2DX.Common.Math.Max(0.0f, _impulse + impulse); - impulse = _impulse - oldImpulse; + float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _pulleyMass * Cdot; + float oldForce = _force; + _force = Box2DXMath.Max(0.0f, _force + force); + force = _force - oldForce; - Vec2 P1 = -impulse * _u1; - Vec2 P2 = -_ratio * impulse * _u2; + Vec2 P1 = -Settings.FORCE_SCALE(step.Dt) * force * _u1; + Vec2 P2 = -Settings.FORCE_SCALE(step.Dt) * _ratio * force * _u2; b1._linearVelocity += b1._invMass * P1; b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1); b2._linearVelocity += b2._invMass * P2; @@ -415,12 +421,12 @@ namespace Box2DX.Dynamics Vec2 v1 = b1._linearVelocity + Vec2.Cross(b1._angularVelocity, r1); float Cdot = -Vec2.Dot(_u1, v1); - float impulse = -_limitMass1 * Cdot; - float oldImpulse = _limitImpulse1; - _limitImpulse1 = Box2DX.Common.Math.Max(0.0f, _limitImpulse1 + impulse); - impulse = _limitImpulse1 - oldImpulse; + float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _limitMass1 * Cdot; + float oldForce = _limitForce1; + _limitForce1 = Box2DXMath.Max(0.0f, _limitForce1 + force); + force = _limitForce1 - oldForce; - Vec2 P1 = -impulse * _u1; + Vec2 P1 = -Settings.FORCE_SCALE(step.Dt) * force * _u1; b1._linearVelocity += b1._invMass * P1; b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1); } @@ -430,18 +436,18 @@ namespace Box2DX.Dynamics Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2); float Cdot = -Vec2.Dot(_u2, v2); - float impulse = -_limitMass2 * Cdot; - float oldImpulse = _limitImpulse2; - _limitImpulse2 = Box2DX.Common.Math.Max(0.0f, _limitImpulse2 + impulse); - impulse = _limitImpulse2 - oldImpulse; + float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _limitMass2 * Cdot; + float oldForce = _limitForce2; + _limitForce2 = Box2DXMath.Max(0.0f, _limitForce2 + force); + force = _limitForce2 - oldForce; - Vec2 P2 = -impulse * _u2; + Vec2 P2 = -Settings.FORCE_SCALE(step.Dt) * force * _u2; b2._linearVelocity += b2._invMass * P2; b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2); } } - internal override bool SolvePositionConstraints(float baumgarte) + internal override bool SolvePositionConstraints() { Body b1 = _body1; Body b2 = _body2; @@ -489,6 +495,9 @@ namespace Box2DX.Dynamics C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -_pulleyMass * C; + float oldImpulse = _positionImpulse; + _positionImpulse = Box2DXMath.Max(0.0f, _positionImpulse + impulse); + impulse = _positionImpulse - oldImpulse; Vec2 P1 = -impulse * _u1; Vec2 P2 = -_ratio * impulse * _u2; @@ -523,6 +532,9 @@ namespace Box2DX.Dynamics linearError = Box2DXMath.Max(linearError, -C); C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -_limitMass1 * C; + float oldLimitPositionImpulse = _limitPositionImpulse1; + _limitPositionImpulse1 = Box2DXMath.Max(0.0f, _limitPositionImpulse1 + impulse); + impulse = _limitPositionImpulse1 - oldLimitPositionImpulse; Vec2 P1 = -impulse * _u1; b1._sweep.C += b1._invMass * P1; @@ -552,6 +564,9 @@ namespace Box2DX.Dynamics linearError = Box2DXMath.Max(linearError, -C); C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -_limitMass2 * C; + float oldLimitPositionImpulse = _limitPositionImpulse2; + _limitPositionImpulse2 = Box2DXMath.Max(0.0f, _limitPositionImpulse2 + impulse); + impulse = _limitPositionImpulse2 - oldLimitPositionImpulse; Vec2 P2 = -impulse * _u2; b2._sweep.C += b2._invMass * P2; @@ -563,4 +578,4 @@ namespace Box2DX.Dynamics return linearError < Settings.LinearSlop; } } -} +} \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/RevoluteJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/RevoluteJoint.cs index e201e01..e337d5d 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/RevoluteJoint.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/RevoluteJoint.cs @@ -76,6 +76,9 @@ namespace Box2DX.Dynamics /// Initialize the bodies, anchors, and reference angle using the world /// anchor. /// + /// + /// + /// public void Initialize(Body body1, Body body2, Vec2 anchor) { Body1 = body1; @@ -144,9 +147,12 @@ namespace Box2DX.Dynamics { public Vec2 _localAnchor1; // relative public Vec2 _localAnchor2; - public Vec3 _impulse; - public float _motorImpulse; - public Mat33 _mass; //effective mass for p2p constraint. + public Vec2 _pivotForce; + public float _motorForce; + public float _limitForce; + public float _limitPositionImpulse; + + public Mat22 _pivotMass; // effective mass for point-to-point constraint. public float _motorMass; // effective mass for motor/limit angular constraint. public bool _enableMotor; @@ -159,6 +165,10 @@ namespace Box2DX.Dynamics public float _upperAngle; public LimitState _limitState; +#if B2_TOI_JOINTS + publiv Vector2 _lastWarmStartingPivotForce; +#endif + public override Vec2 Anchor1 { get { return _body1.GetWorldPoint(_localAnchor1); } @@ -169,15 +179,14 @@ namespace Box2DX.Dynamics get { return _body2.GetWorldPoint(_localAnchor2); } } - public override Vec2 GetReactionForce(float inv_dt) + public override Vec2 ReactionForce { - Vec2 P = new Vec2(_impulse.X, _impulse.Y); - return inv_dt * P; + get { return Settings.FORCE_SCALE(1.0f) * _pivotForce; } } - public override float GetReactionTorque(float inv_dt) + public override float ReactionTorque { - return inv_dt * _impulse.Z; + get { return _limitForce; } } /// @@ -218,10 +227,9 @@ namespace Box2DX.Dynamics /// /// Enable/disable the joint limit. /// + /// public void EnableLimit(bool flag) { - _body1.WakeUp(); - _body2.WakeUp(); _enableLimit = flag; } @@ -244,11 +252,11 @@ namespace Box2DX.Dynamics /// /// Set the joint limits in radians. /// + /// + /// public void SetLimits(float lower, float upper) { Box2DXDebug.Assert(lower <= upper); - _body1.WakeUp(); - _body2.WakeUp(); _lowerAngle = lower; _upperAngle = upper; } @@ -264,10 +272,9 @@ namespace Box2DX.Dynamics /// /// Enable/disable the joint motor. /// + /// public void EnableMotor(bool flag) { - _body1.WakeUp(); - _body2.WakeUp(); _enableMotor = flag; } @@ -277,30 +284,22 @@ namespace Box2DX.Dynamics public float MotorSpeed { get { return _motorSpeed; } - set - { - _body1.WakeUp(); - _body2.WakeUp(); - _motorSpeed = value; - } + set { _motorSpeed = value; } } /// /// Set the maximum motor torque, usually in N-m. /// + /// public void SetMaxMotorTorque(float torque) { - _body1.WakeUp(); - _body2.WakeUp(); _maxMotorTorque = torque; } - /// /// Get the current motor torque, usually in N-m. - /// public float MotorTorque { - get { return _motorImpulse; } + get { return _motorForce; } } public RevoluteJoint(RevoluteJointDef def) @@ -310,8 +309,10 @@ namespace Box2DX.Dynamics _localAnchor2 = def.LocalAnchor2; _referenceAngle = def.ReferenceAngle; - _impulse = new Vec3(); - _motorImpulse = 0.0f; + _pivotForce.Set(0.0f, 0.0f); + _motorForce = 0.0f; + _limitForce = 0.0f; + _limitPositionImpulse = 0.0f; _lowerAngle = def.LowerAngle; _upperAngle = def.UpperAngle; @@ -319,7 +320,6 @@ namespace Box2DX.Dynamics _motorSpeed = def.MotorSpeed; _enableLimit = def.EnableLimit; _enableMotor = def.EnableMotor; - _limitState = LimitState.InactiveLimit; } internal override void InitVelocityConstraints(TimeStep step) @@ -327,44 +327,36 @@ namespace Box2DX.Dynamics Body b1 = _body1; Body b2 = _body2; - if (_enableMotor || _enableLimit) - { - // You cannot create a rotation limit between bodies that - // both have fixed rotation. - Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f); - } - // Compute the effective mass matrix. Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] + // 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] + float invMass1 = b1._invMass, invMass2 = b2._invMass; + float invI1 = b1._invI, invI2 = b2._invI; - // 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] + Mat22 K1 = new Mat22(); + K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f; + K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2; - float m1 = b1._invMass, m2 = b2._invMass; - float i1 = b1._invI, i2 = b2._invI; + Mat22 K2 = new Mat22(); + 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; - _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2; - _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2; - _mass.Col3.X = -r1.Y * i1 - r2.Y * i2; - _mass.Col1.Y = _mass.Col2.X; - _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2; - _mass.Col3.Y = r1.X * i1 + r2.X * i2; - _mass.Col1.Z = _mass.Col3.X; - _mass.Col2.Z = _mass.Col3.Y; - _mass.Col3.Z = i1 + i2; + Mat22 K3 = new Mat22(); + 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; - _motorMass = 1.0f / (i1 + i2); + Mat22 K = K1 + K2 + K3; + _pivotMass = K.Invert(); + + _motorMass = 1.0f / (invI1 + invI2); if (_enableMotor == false) { - _motorImpulse = 0.0f; + _motorForce = 0.0f; } if (_enableLimit) @@ -378,7 +370,7 @@ namespace Box2DX.Dynamics { if (_limitState != LimitState.AtLowerLimit) { - _impulse.Z = 0.0f; + _limitForce = 0.0f; } _limitState = LimitState.AtLowerLimit; } @@ -386,40 +378,37 @@ namespace Box2DX.Dynamics { if (_limitState != LimitState.AtUpperLimit) { - _impulse.Z = 0.0f; + _limitForce = 0.0f; } _limitState = LimitState.AtUpperLimit; } else { _limitState = LimitState.InactiveLimit; - _impulse.Z = 0.0f; + _limitForce = 0.0f; } } else { - _limitState = LimitState.InactiveLimit; + _limitForce = 0.0f; } if (step.WarmStarting) { - // Scale impulses to support a variable time step. - _impulse *= step.DtRatio; - _motorImpulse *= step.DtRatio; - - Vec2 P = new Vec2(_impulse.X, _impulse.Y); - - b1._linearVelocity -= m1 * P; - b1._angularVelocity -= i1 * (Vec2.Cross(r1, P) + _motorImpulse + _impulse.Z); + b1._linearVelocity -= Settings.FORCE_SCALE(step.Dt) * invMass1 * _pivotForce; + b1._angularVelocity -= Settings.FORCE_SCALE(step.Dt) * invI1 * (Vec2.Cross(r1, _pivotForce) + Settings.FORCE_INV_SCALE(_motorForce + _limitForce)); - b2._linearVelocity += m2 * P; - b2._angularVelocity += i2 * (Vec2.Cross(r2, P) + _motorImpulse + _impulse.Z); + b2._linearVelocity += Settings.FORCE_SCALE(step.Dt) * invMass2 * _pivotForce; + b2._angularVelocity += Settings.FORCE_SCALE(step.Dt) * invI2 * (Vec2.Cross(r2, _pivotForce) + Settings.FORCE_INV_SCALE(_motorForce + _limitForce)); } else { - _impulse.SetZero(); - _motorImpulse = 0.0f; + _pivotForce.SetZero(); + _motorForce = 0.0f; + _limitForce = 0.0f; } + + _limitPositionImpulse = 0.0f; } internal override void SolveVelocityConstraints(TimeStep step) @@ -427,119 +416,129 @@ namespace Box2DX.Dynamics Body b1 = _body1; Body b2 = _body2; - Vec2 v1 = b1._linearVelocity; - float w1 = b1._angularVelocity; - Vec2 v2 = b2._linearVelocity; - float w2 = b2._angularVelocity; + Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); - float m1 = b1._invMass, m2 = b2._invMass; - float i1 = b1._invI, i2 = b2._invI; + // Solve point-to-point constraint + Vec2 pivotCdot = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2) - b1._linearVelocity - + Vec2.Cross(b1._angularVelocity, r1); + Vec2 pivotForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * Box2DXMath.Mul(_pivotMass, pivotCdot); + +#if B2_TOI_JOINTS + if (step.WarmStarting) + { + _pivotForce += pivotForce; + _lastWarmStartingPivotForce = _pivotForce; + } + else + { + _pivotForce = _lastWarmStartingPivotForce; + //Do not update warm starting value! + } +#else + _pivotForce += pivotForce; +#endif + + Vec2 P = Settings.FORCE_SCALE(step.Dt) * pivotForce; + b1._linearVelocity -= b1._invMass * P; + b1._angularVelocity -= b1._invI * Vec2.Cross(r1, P); + + b2._linearVelocity += b2._invMass * P; + b2._angularVelocity += b2._invI * Vec2.Cross(r2, P); - //Solve motor constraint. if (_enableMotor && _limitState != LimitState.EqualLimits) { - float Cdot = w2 - w1 - _motorSpeed; - float impulse = _motorMass * (-Cdot); - float oldImpulse = _motorImpulse; - float maxImpulse = step.Dt * _maxMotorTorque; - _motorImpulse = Box2DXMath.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = _motorImpulse - oldImpulse; - - w1 -= i1 * impulse; - w2 += i2 * impulse; + float motorCdot = b2._angularVelocity - b1._angularVelocity - _motorSpeed; + float motorForce = -step.Inv_Dt * _motorMass * motorCdot; + float oldMotorForce = _motorForce; + _motorForce = Box2DXMath.Clamp(_motorForce + motorForce, -_maxMotorTorque, _maxMotorTorque); + motorForce = _motorForce - oldMotorForce; + + float P_ = step.Dt * motorForce; + b1._angularVelocity -= b1._invI * P_; + b2._angularVelocity += b2._invI * P_; } - //Solve limit constraint. if (_enableLimit && _limitState != LimitState.InactiveLimit) { - Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); - Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); - - // Solve point-to-point constraint - Vec2 Cdot1 = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1); - float Cdot2 = w2 - w1; - Vec3 Cdot = new Vec3(Cdot1.X, Cdot1.Y, Cdot2); - - Vec3 impulse = _mass.Solve33(-Cdot); + float limitCdot = b2._angularVelocity - b1._angularVelocity; + float limitForce = -step.Inv_Dt * _motorMass * limitCdot; if (_limitState == LimitState.EqualLimits) { - _impulse += impulse; + _limitForce += limitForce; } else if (_limitState == LimitState.AtLowerLimit) { - float newImpulse = _impulse.Z + impulse.Z; - if (newImpulse < 0.0f) - { - Vec2 reduced = _mass.Solve22(-Cdot1); - impulse.X = reduced.X; - impulse.Y = reduced.Y; - impulse.Z = -_impulse.Z; - _impulse.X += reduced.X; - _impulse.Y += reduced.Y; - _impulse.Z = 0.0f; - } + float oldLimitForce = _limitForce; + _limitForce = Box2DXMath.Max(_limitForce + limitForce, 0.0f); + limitForce = _limitForce - oldLimitForce; } else if (_limitState == LimitState.AtUpperLimit) { - float newImpulse = _impulse.Z + impulse.Z; - if (newImpulse > 0.0f) - { - Vec2 reduced = _mass.Solve22(-Cdot1); - impulse.X = reduced.X; - impulse.Y = reduced.Y; - impulse.Z = -_impulse.Z; - _impulse.X += reduced.X; - _impulse.Y += reduced.Y; - _impulse.Z = 0.0f; - } + float oldLimitForce = _limitForce; + _limitForce = Box2DXMath.Min(_limitForce + limitForce, 0.0f); + limitForce = _limitForce - oldLimitForce; } - Vec2 P = new Vec2(impulse.X, impulse.Y); + float P_ = step.Dt * limitForce; + b1._angularVelocity -= b1._invI * P_; + b2._angularVelocity += b2._invI * P_; + } + } + + internal override bool SolvePositionConstraints() + { + Body b1 = _body1; + Body b2 = _body2; + + float positionError = 0.0f; - v1 -= m1 * P; - w1 -= i1 * (Vec2.Cross(r1, P) + impulse.Z); + // Solve point-to-point position error. + Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); - v2 += m2 * P; - w2 += i2 * (Vec2.Cross(r2, P) + impulse.Z); - } - else - { - Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); - Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + Vec2 p1 = b1._sweep.C + r1; + Vec2 p2 = b2._sweep.C + r2; + Vec2 ptpC = p2 - p1; - // Solve point-to-point constraint - Vec2 Cdot = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1); - Vec2 impulse = _mass.Solve22(-Cdot); + positionError = ptpC.Length(); - _impulse.X += impulse.X; - _impulse.Y += impulse.Y; + // Prevent overly large corrections. + //b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection); + //ptpC = b2Clamp(ptpC, -dpMax, dpMax); - v1 -= m1 * impulse; - w1 -= i1 * Vec2.Cross(r1, impulse); + float invMass1 = b1._invMass, invMass2 = b2._invMass; + float invI1 = b1._invI, invI2 = b2._invI; - v2 += m2 * impulse; - w2 += i2 * Vec2.Cross(r2, impulse); - } + Mat22 K1 = new Mat22(); + K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f; + K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2; - b1._linearVelocity = v1; - b1._angularVelocity = w1; - b2._linearVelocity = v2; - b2._angularVelocity = w2; - } + Mat22 K2 = new Mat22(); + 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; - internal override bool SolvePositionConstraints(float baumgarte) - { - // TODO_ERIN block solve with limit. + Mat22 K3 = new Mat22(); + 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; - Body b1 = _body1; - Body b2 = _body2; + Mat22 K = K1 + K2 + K3; + Vec2 impulse = K.Solve(-ptpC); + + b1._sweep.C -= b1._invMass * impulse; + b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse); + b2._sweep.C += b2._invMass * impulse; + b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse); + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + + // Handle limits. float angularError = 0.0f; - float positionError = 0.0f; - // Solve angular limit constraint. - if (_enableLimit && _limitState != LimitState.InactiveLimit) + if (_enableLimit && _limitState != LimitState.InactiveLimit) { float angle = b2._sweep.A - b1._sweep.A - _referenceAngle; float limitImpulse = 0.0f; @@ -547,27 +546,33 @@ namespace Box2DX.Dynamics if (_limitState == LimitState.EqualLimits) { // Prevent large angular corrections - float C = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); - limitImpulse = -_motorMass * C; - angularError = Box2DXMath.Abs(C); + float limitC = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); + limitImpulse = -_motorMass * limitC; + angularError = Box2DXMath.Abs(limitC); } else if (_limitState == LimitState.AtLowerLimit) { - float C = angle - _lowerAngle; - angularError = -C; + float limitC = angle - _lowerAngle; + angularError = Box2DXMath.Max(0.0f, -limitC); // Prevent large angular corrections and allow some slop. - C = Box2DXMath.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); - limitImpulse = -_motorMass * C; + limitC = Box2DXMath.Clamp(limitC + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); + limitImpulse = -_motorMass * limitC; + float oldLimitImpulse = _limitPositionImpulse; + _limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f); + limitImpulse = _limitPositionImpulse - oldLimitImpulse; } else if (_limitState == LimitState.AtUpperLimit) { - float C = angle - _upperAngle; - angularError = C; + float limitC = angle - _upperAngle; + angularError = Box2DXMath.Max(0.0f, limitC); // Prevent large angular corrections and allow some slop. - C = Box2DXMath.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); - limitImpulse = -_motorMass * C; + limitC = Box2DXMath.Clamp(limitC - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); + limitImpulse = -_motorMass * limitC; + float oldLimitImpulse = _limitPositionImpulse; + _limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f); + limitImpulse = _limitPositionImpulse - oldLimitImpulse; } b1._sweep.A -= b1._invI * limitImpulse; @@ -577,60 +582,7 @@ namespace Box2DX.Dynamics b2.SynchronizeTransform(); } - // Solve point-to-point constraint. - { - Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); - Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); - - Vec2 C = b2._sweep.C + r2 - b1._sweep.C - r1; - positionError = C.Length(); - - float invMass1 = b1._invMass, invMass2 = b2._invMass; - float invI1 = b1._invI, invI2 = b2._invI; - - // Handle large detachment. - float k_allowedStretch = 10.0f * Settings.LinearSlop; - if (C.LengthSquared() > k_allowedStretch * k_allowedStretch) - { - // Use a particle solution (no rotation). - Vec2 u = C; u.Normalize(); - float k = invMass1 + invMass2; - Box2DXDebug.Assert(k > Settings.FLT_EPSILON); - float m = 1.0f / k; - Vec2 impulse = m * (-C); - float k_beta = 0.5f; - b1._sweep.C -= k_beta * invMass1 * impulse; - b2._sweep.C += k_beta * invMass2 * impulse; - - C = b2._sweep.C + r2 - b1._sweep.C - r1; - } - - Mat22 K1 = new Mat22(); - K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f; - K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2; - - Mat22 K2 = new Mat22(); - 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; - - Mat22 K3 = new Mat22(); - 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; - - Mat22 K = K1 + K2 + K3; - Vec2 impulse_ = K.Solve(-C); - - b1._sweep.C -= b1._invMass * impulse_; - b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse_); - - b2._sweep.C += b2._invMass * impulse_; - b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse_); - - b1.SynchronizeTransform(); - b2.SynchronizeTransform(); - } - return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; } } -} +} \ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/World.cs b/Box2d/Assets/Program/Box2d/Dynamics/World.cs index 7ebc5eb..39f1308 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/World.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/World.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -20,6 +20,8 @@ */ using System; +using System.Collections.Generic; +using System.Text; using Box2DX.Common; using Box2DX.Collision; @@ -33,7 +35,7 @@ namespace Box2DX.Dynamics public float DtRatio; // dt * inv_dt0 public int VelocityIterations; public int PositionIterations; - public bool WarmStarting; + public bool WarmStarting; } /// @@ -49,7 +51,6 @@ namespace Box2DX.Dynamics private Body _bodyList; private Joint _jointList; - private Controllers.Controller _controllerList; private Vec2 _raycastNormal; private object _raycastUserData; @@ -62,7 +63,6 @@ namespace Box2DX.Dynamics private int _bodyCount; internal int _contactCount; private int _jointCount; - private int _controllerCount; private Vec2 _gravity; /// @@ -100,7 +100,7 @@ namespace Box2DX.Dynamics { _destructionListener = null; _boundaryListener = null; - _contactFilter = null; + _contactFilter = WorldCallback.DefaultFilter; _contactListener = null; _debugDraw = null; @@ -252,30 +252,23 @@ namespace Box2DX.Dynamics DestroyJoint(jn0.Joint); } - //Detach controllers attached to this body - Controllers.ControllerEdge ce = b._controllerList; - while (ce != null) - { - Controllers.ControllerEdge ce0 = ce; - ce = ce.nextController; - - ce0.controller.RemoveBody(b); - } - - // Delete the attached fixtures. This destroys broad-phase + // Delete the attached shapes. This destroys broad-phase // proxies and pairs, leading to the destruction of contacts. - Fixture f = b._fixtureList; - while (f != null) + Shape s = null; + if (b._shapeList != null) + s = b._shapeList; + while (s != null) { - Fixture f0 = f; - f = f.Next; + Shape s0 = s; + s = s._next; if (_destructionListener != null) { - _destructionListener.SayGoodbye(f0); + _destructionListener.SayGoodbye(s0); } - f0.Destroy(_broadPhase); + s0.DestroyProxy(_broadPhase); + Shape.Destroy(s0); } // Remove world body list. @@ -344,10 +337,10 @@ namespace Box2DX.Dynamics if (def.CollideConnected == false) { // Reset the proxies on the body with the minimum number of shapes. - Body b = def.Body1._fixtureCount < def.Body2._fixtureCount ? def.Body1 : def.Body2; - for (Fixture f = b._fixtureList; f != null; f = f.Next) + Body b = def.Body1._shapeCount < def.Body2._shapeCount ? def.Body1 : def.Body2; + for (Shape s = b._shapeList; s != null; s = s._next) { - f.RefilterProxy(_broadPhase, b.GetXForm()); + s.RefilterProxy(_broadPhase, b.GetXForm()); } } @@ -436,40 +429,14 @@ namespace Box2DX.Dynamics if (collideConnected == false) { // Reset the proxies on the body with the minimum number of shapes. - Body b = body1._fixtureCount < body2._fixtureCount ? body1 : body2; - for (Fixture f = b._fixtureList; f != null; f = f.Next) + Body b = body1._shapeCount < body2._shapeCount ? body1 : body2; + for (Shape s = b._shapeList; s != null; s = s._next) { - f.RefilterProxy(_broadPhase, b.GetXForm()); + s.RefilterProxy(_broadPhase, b.GetXForm()); } } } - public Controllers.Controller AddController(Controllers.Controller def) - { - def._next = _controllerList; - def._prev = null; - if (_controllerList != null) - _controllerList._prev = def; - _controllerList = def; - ++_controllerCount; - - def._world = this; - - return def; - } - - public void RemoveController(Controllers.Controller controller) - { - Box2DXDebug.Assert(_controllerCount > 0); - if (controller._next != null) - controller._next._prev = controller._prev; - if (controller._prev != null) - controller._prev._next = controller._next; - if (controller == _controllerList) - _controllerList = controller._next; - --_controllerCount; - } - /// /// The world provides a single static ground body with no collision shapes. /// You can use this to simplify the creation of joints and static shapes. @@ -500,23 +467,13 @@ namespace Box2DX.Dynamics return _jointList; } - public Controllers.Controller GetControllerList() - { - return _controllerList; - } - - public int GetControllerCount() - { - return _controllerCount; - } - /// - /// Re-filter a fixture. This re-runs contact filtering on a fixture. + /// Re-filter a shape. This re-runs contact filtering on a shape. /// - public void Refilter(Fixture fixture) + public void Refilter(Shape shape) { Box2DXDebug.Assert(_lock == false); - fixture.RefilterProxy(_broadPhase, fixture.Body.GetXForm()); + shape.RefilterProxy(_broadPhase, shape.GetBody().GetXForm()); } /// @@ -620,7 +577,7 @@ namespace Box2DX.Dynamics /// @param shapes a user allocated shape pointer array of size maxCount (or greater). /// @param maxCount the capacity of the shapes array. /// @return the number of shapes found in aabb. - public int Query(AABB aabb, Fixture[] fixtures, int maxCount) + public int Query(AABB aabb, Shape[] shapes, int maxCount) { //using (object[] results = new object[maxCount]) { @@ -630,7 +587,7 @@ namespace Box2DX.Dynamics for (int i = 0; i < count; ++i) { - fixtures[i] = (Fixture)results[i]; + shapes[i] = (Shape)results[i]; } results = null; @@ -650,7 +607,7 @@ namespace Box2DX.Dynamics /// Determines if shapes that the ray starts in are counted as hits. /// Passed through the worlds contact filter, with method RayCollide. This can be used to filter valid shapes. /// The number of shapes found - public int Raycast(Segment segment, out Fixture[] fixtures, int maxCount, bool solidShapes, object userData) + public int Raycast(Segment segment, Shape[] shapes, int maxCount, bool solidShapes, object userData) { #warning "PTR" _raycastSegment = segment; @@ -658,14 +615,12 @@ namespace Box2DX.Dynamics _raycastSolidShape = solidShapes; object[] results = new object[maxCount]; - fixtures = new Fixture[maxCount]; int count = _broadPhase.QuerySegment(segment, results, maxCount, RaycastSortKey); - for (int i = 0; i < count; ++i) { - fixtures[i] = (Fixture)results[i]; + shapes[i] = (Shape)results[i]; } - + results = null; return count; } @@ -680,14 +635,15 @@ namespace Box2DX.Dynamics /// Determines if shapes that the ray starts in are counted as hits. /// /// Returns the colliding shape shape, or null if not found. - public Fixture RaycastOne(Segment segment, out float lambda, out Vec2 normal, bool solidShapes, object userData) + public Shape RaycastOne(Segment segment, out float lambda, out Vec2 normal, bool solidShapes, object userData) { + lambda = 0; + normal = new Vec2(0,0); + int maxCount = 1; - Fixture[] fixture; - lambda = 0.0f; - normal = new Vec2(); + Shape[] shape = new Shape[maxCount]; - int count = Raycast(segment, out fixture, maxCount, solidShapes, userData); + int count = Raycast(segment, shape, maxCount, solidShapes, userData); if (count == 0) return null; @@ -696,20 +652,15 @@ namespace Box2DX.Dynamics //Redundantly do TestSegment a second time, as the previous one's results are inaccessible - fixture[0].TestSegment(out lambda, out normal, segment, 1); + XForm xf = shape[0].GetBody().GetXForm(); + shape[0].TestSegment(xf, out lambda, out normal, segment, 1); //We already know it returns true - return fixture[0]; + return shape[0]; } // Find islands, integrate and solve constraints, solve position constraints private void Solve(TimeStep step) { - // Step all controlls - for (Controllers.Controller controller = _controllerList; controller != null; controller = controller._next) - { - controller.Step(step); - } - // Size the island for the worst case. Island island = new Island(_bodyCount, _contactCount, _jointCount, _contactListener); @@ -777,7 +728,7 @@ namespace Box2DX.Dynamics } // Is this contact touching? - if ((cn.Contact._flags & Contact.CollisionFlags.Touch) == (Contact.CollisionFlags)0) + if (cn.Contact.GetManifoldCount() == 0) { continue; } @@ -854,7 +805,7 @@ namespace Box2DX.Dynamics // Update shapes (for broad-phase). If the shapes go out of // the world AABB then shapes and contacts may be destroyed, // including contacts that are - bool inRange = b.SynchronizeFixtures(); + bool inRange = b.SynchronizeShapes(); // Did the body's shapes leave the world? if (inRange == false && _boundaryListener != null) @@ -884,7 +835,6 @@ namespace Box2DX.Dynamics // --queueSize; int queueCapacity = _bodyCount; Body[] queue = new Body[queueCapacity]; - for (Body b = _bodyList; b != null; b = b._next) { b._flags &= ~Body.BodyFlags.Island; @@ -897,10 +847,12 @@ namespace Box2DX.Dynamics c._flags &= ~(Contact.CollisionFlags.Toi | Contact.CollisionFlags.Island); } - for (Joint j = _jointList; j != null; j = j._next) +#if B2_TOI_JOINTS + for (Joint j = _jointList; j!=null; j = j._next) { - j._islandFlag = false; + j._islandFlag = false; } +#endif // Find TOI events and solve them. for (; ; ) @@ -911,7 +863,7 @@ namespace Box2DX.Dynamics for (Contact c = _contactList; c != null; c = c._next) { - if ((int)(c._flags & (Contact.CollisionFlags.Slow | Contact.CollisionFlags.NonSolid)) == 1) + if ((c._flags & (Contact.CollisionFlags.Slow | Contact.CollisionFlags.NonSolid)) != 0) { continue; } @@ -919,7 +871,7 @@ namespace Box2DX.Dynamics // TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact. float toi = 1.0f; - if ((int)(c._flags & Contact.CollisionFlags.Toi) == 1) + if ((c._flags & Contact.CollisionFlags.Toi) != 0) { // This contact has a valid cached TOI. toi = c._toi; @@ -927,42 +879,38 @@ namespace Box2DX.Dynamics else { // Compute the TOI for this contact. - Fixture s1 = c.FixtureA; - Fixture s2 = c.FixtureB; - Body b1 = s1.Body; - Body b2 = s2.Body; + Shape s1_ = c.GetShape1(); + Shape s2_ = c.GetShape2(); + Body b1_ = s1_.GetBody(); + Body b2_ = s2_.GetBody(); - if ((b1.IsStatic() || b1.IsSleeping()) && (b2.IsStatic() || b2.IsSleeping())) + if ((b1_.IsStatic() || b1_.IsSleeping()) && (b2_.IsStatic() || b2_.IsSleeping())) { continue; } // Put the sweeps onto the same time interval. - float t0 = b1._sweep.T0; + float t0 = b1_._sweep.T0; - if (b1._sweep.T0 < b2._sweep.T0) + if (b1_._sweep.T0 < b2_._sweep.T0) { - t0 = b2._sweep.T0; - b1._sweep.Advance(t0); + t0 = b2_._sweep.T0; + b1_._sweep.Advance(t0); } - else if (b2._sweep.T0 < b1._sweep.T0) + else if (b2_._sweep.T0 < b1_._sweep.T0) { - t0 = b1._sweep.T0; - b2._sweep.Advance(t0); + t0 = b1_._sweep.T0; + b2_._sweep.Advance(t0); } Box2DXDebug.Assert(t0 < 1.0f); // Compute the time of impact. - toi = c.ComputeTOI(b1._sweep, b2._sweep); - //b2TimeOfImpact(c->m_fixtureA->GetShape(), b1->m_sweep, c->m_fixtureB->GetShape(), b2->m_sweep); - + toi = Collision.Collision.TimeOfImpact(c._shape1, b1_._sweep, c._shape2, b2_._sweep); Box2DXDebug.Assert(0.0f <= toi && toi <= 1.0f); - // If the TOI is in range ... - if (0.0f < toi && toi < 1.0f) + if (toi > 0.0f && toi < 1.0f) { - // Interpolate on the actual range. toi = Common.Math.Min((1.0f - toi) * t0 + toi, 1.0f); } @@ -971,7 +919,7 @@ namespace Box2DX.Dynamics c._flags |= Contact.CollisionFlags.Toi; } - if (Settings.FLT_EPSILON < toi && toi < minTOI) + if (Common.Settings.FLT_EPSILON < toi && toi < minTOI) { // This is the minimum TOI found so far. minContact = c; @@ -979,25 +927,25 @@ namespace Box2DX.Dynamics } } - if (minContact == null || 1.0f - 100.0f * Settings.FLT_EPSILON < minTOI) + if (minContact == null || 1.0f - 100.0f * Common.Settings.FLT_EPSILON < minTOI) { // No more TOI events. Done! break; } // Advance the bodies to the TOI. - Fixture f1 = minContact.FixtureA; - Fixture f2 = minContact.FixtureB; - Body b3 = f1.Body; - Body b4 = f2.Body; - b3.Advance(minTOI); - b4.Advance(minTOI); + Shape s1 = minContact.GetShape1(); + Shape s2 = minContact.GetShape2(); + Body b1 = s1.GetBody(); + Body b2 = s2.GetBody(); + b1.Advance(minTOI); + b2.Advance(minTOI); // The TOI contact likely has some new contact points. minContact.Update(_contactListener); minContact._flags &= ~Contact.CollisionFlags.Toi; - if ((minContact._flags & Contact.CollisionFlags.Touch) == 0) + if (minContact.GetManifoldCount() == 0) { // This shouldn't happen. Numerical error? //b2Assert(false); @@ -1005,17 +953,17 @@ namespace Box2DX.Dynamics } // Build the TOI island. We need a dynamic seed. - Body seed = b3; + Body seed = b1; if (seed.IsStatic()) { - seed = b4; + seed = b2; } // Reset island and queue. island.Clear(); - int queueStart = 0; // starting index for queue - int queueSize = 0; // elements in queue + int queueStart = 0; //starting index for queue + int queueSize = 0; //elements in queue queue[queueStart + queueSize++] = seed; seed._flags |= Body.BodyFlags.Island; @@ -1025,7 +973,6 @@ namespace Box2DX.Dynamics // Grab the next body off the stack and add it to the island. Body b = queue[queueStart++]; --queueSize; - island.Add(b); // Make sure the body is awake. @@ -1039,7 +986,7 @@ namespace Box2DX.Dynamics } // Search all contacts connected to this body. - for (ContactEdge cEdge = b._contactList; cEdge != null; cEdge = cEdge.Next) + for (ContactEdge cn = b._contactList; cn != null; cn = cn.Next) { // Does the TOI island still have space for contacts? if (island._contactCount == island._contactCapacity) @@ -1048,25 +995,25 @@ namespace Box2DX.Dynamics } // Has this contact already been added to an island? Skip slow or non-solid contacts. - if ((int)(cEdge.Contact._flags & (Contact.CollisionFlags.Island | Contact.CollisionFlags.Slow | Contact.CollisionFlags.NonSolid)) != 0) + if ((cn.Contact._flags & (Contact.CollisionFlags.Island | Contact.CollisionFlags.Slow | Contact.CollisionFlags.NonSolid)) != 0) { continue; } // Is this contact touching? For performance we are not updating this contact. - if ((cEdge.Contact._flags & Contact.CollisionFlags.Touch) == 0) + if (cn.Contact.GetManifoldCount() == 0) { continue; } - island.Add(cEdge.Contact); - cEdge.Contact._flags |= Contact.CollisionFlags.Island; + island.Add(cn.Contact); + cn.Contact._flags |= Contact.CollisionFlags.Island; // Update other body. - Body other = cEdge.Other; + Body other = cn.Other; // Was the other body already added to this island? - if ((int)(other._flags & Body.BodyFlags.Island) == 1) + if ((other._flags & Body.BodyFlags.Island) != 0) { continue; } @@ -1078,53 +1025,53 @@ namespace Box2DX.Dynamics other.WakeUp(); } - //Box2DXDebug.Assert(queueStart + queueSize < queueCapacity); - queue[queueStart + queueSize] = other; - ++queueSize; + Box2DXDebug.Assert(queueStart + queueSize < queueCapacity); + queue[queueStart + queueSize++] = other; other._flags |= Body.BodyFlags.Island; } - for (JointEdge jEdge = b._jointList; jEdge != null; jEdge = jEdge.Next) +#if B2_TOI_JOINTS + for (JointEdge jn = b._jointList; jn!=null; jn = jn.Next) { if (island._jointCount == island._jointCapacity) { continue; } - - if (jEdge.Joint._islandFlag == true) + + if (jn.Joint._islandFlag == true) { continue; } - - island.Add(jEdge.Joint); - - jEdge.Joint._islandFlag = true; - - Body other = jEdge.Other; - - if ((int)(other._flags & Body.BodyFlags.Island) == 1) + + island.Add(jn.Joint); + + jn.Joint._islandFlag = true; + + Body other = jn.Other; + + if (other._flags & Body.BodyFlags.Island) { continue; } - + if (!other.IsStatic()) { other.Advance(minTOI); other.WakeUp(); } - - //Box2DXDebug.Assert(queueStart + queueSize < queueCapacity); - queue[queueStart + queueSize] = other; - ++queueSize; - other._flags |= Body.BodyFlags.Island; + + Box2DXDebug.Assert(queueStart + queueSize < queueCapacity); + queue[queueStart + queueSize++] = other; + other._flags |= Body.BodyFlags.Island; } +#endif } - TimeStep subStep; + TimeStep subStep = new TimeStep(); subStep.WarmStarting = false; subStep.Dt = (1.0f - minTOI) * step.Dt; + Box2DXDebug.Assert(subStep.Dt > Common.Settings.FLT_EPSILON); subStep.Inv_Dt = 1.0f / subStep.Dt; - subStep.DtRatio = 0.0f; subStep.VelocityIterations = step.VelocityIterations; subStep.PositionIterations = step.PositionIterations; @@ -1137,7 +1084,7 @@ namespace Box2DX.Dynamics Body b = island._bodies[i]; b._flags &= ~Body.BodyFlags.Island; - if ((int)(b._flags & (Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) == 1) + if ((b._flags & (Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) != 0) { continue; } @@ -1147,12 +1094,12 @@ namespace Box2DX.Dynamics continue; } - // Update fixtures (for broad-phase). If the fixtures go out of - // the world AABB then fixtures and contacts may be destroyed, + // Update shapes (for broad-phase). If the shapes go out of + // the world AABB then shapes and contacts may be destroyed, // including contacts that are - bool inRange = b.SynchronizeFixtures(); + bool inRange = b.SynchronizeShapes(); - // Did the body's fixtures leave the world? + // Did the body's shapes leave the world? if (inRange == false && _boundaryListener != null) { _boundaryListener.Violation(b); @@ -1180,7 +1127,7 @@ namespace Box2DX.Dynamics j._islandFlag = false; } - // Commit fixture proxy movements to the broad-phase so that new contacts are created. + // Commit shape proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. _broadPhase.Commit(); } @@ -1230,30 +1177,34 @@ namespace Box2DX.Dynamics } } - private void DrawFixture(Fixture fixture, XForm xf, Color color, bool core) + private void DrawShape(Shape shape, XForm xf, Color color, bool core) { -#warning "the core argument is not used, the coreColor variable is also not used" Color coreColor = new Color(0.9f, 0.6f, 0.6f); - switch (fixture.ShapeType) + switch (shape.GetType()) { case ShapeType.CircleShape: { - CircleShape circle = (CircleShape)fixture.Shape; + CircleShape circle = (CircleShape)shape; - Vec2 center = Common.Math.Mul(xf, circle._position); - float radius = circle._radius; + Vec2 center = Common.Math.Mul(xf, circle.GetLocalPosition()); + float radius = circle.GetRadius(); Vec2 axis = xf.R.Col1; _debugDraw.DrawSolidCircle(center, radius, axis, color); + + if (core) + { + _debugDraw.DrawCircle(center, radius - Settings.ToiSlop, coreColor); + } } break; case ShapeType.PolygonShape: { - PolygonShape poly = (PolygonShape)fixture.Shape; - int vertexCount = poly._vertexCount; - Vec2[] localVertices = poly._vertices; + PolygonShape poly = (PolygonShape)shape; + int vertexCount = poly.VertexCount; + Vec2[] localVertices = poly.GetVertices(); Box2DXDebug.Assert(vertexCount <= Settings.MaxPolygonVertices); Vec2[] vertices = new Vec2[Settings.MaxPolygonVertices]; @@ -1264,14 +1215,16 @@ namespace Box2DX.Dynamics } _debugDraw.DrawSolidPolygon(vertices, vertexCount, color); - } - break; - case ShapeType.EdgeShape: - { - EdgeShape edge = (EdgeShape)fixture.Shape; - - _debugDraw.DrawSegment(Common.Math.Mul(xf, edge.Vertex1), Common.Math.Mul(xf, edge.Vertex2), color); + if (core) + { + Vec2[] localCoreVertices = poly.GetCoreVertices(); + for (int i = 0; i < vertexCount; ++i) + { + vertices[i] = Common.Math.Mul(xf, localCoreVertices[i]); + } + _debugDraw.DrawPolygon(vertices, vertexCount, coreColor); + } } break; } @@ -1293,19 +1246,19 @@ namespace Box2DX.Dynamics for (Body b = _bodyList; b != null; b = b.GetNext()) { XForm xf = b.GetXForm(); - for (Fixture f = b.GetFixtureList(); f != null; f = f.Next) + for (Shape s = b.GetShapeList(); s != null; s = s.GetNext()) { if (b.IsStatic()) { - DrawFixture(f, xf, new Color(0.5f, 0.9f, 0.5f), core); + DrawShape(s, xf, new Color(0.5f, 0.9f, 0.5f), core); } else if (b.IsSleeping()) { - DrawFixture(f, xf, new Color(0.5f, 0.5f, 0.9f), core); + DrawShape(s, xf, new Color(0.5f, 0.5f, 0.9f), core); } else { - DrawFixture(f, xf, new Color(0.9f, 0.9f, 0.9f), core); + DrawShape(s, xf, new Color(0.9f, 0.9f, 0.9f), core); } } } @@ -1322,14 +1275,6 @@ namespace Box2DX.Dynamics } } - if ((flags & DebugDraw.DrawFlags.Controller) != 0) - { - for (Controllers.Controller c = _controllerList; c != null; c = c.GetNext()) - { - c.Draw(_debugDraw); - } - } - if ((flags & DebugDraw.DrawFlags.Pair) != 0) { BroadPhase bp = _broadPhase; @@ -1406,6 +1351,40 @@ namespace Box2DX.Dynamics _debugDraw.DrawPolygon(vs, 4, new Color(0.3f, 0.9f, 0.9f)); } + if ((flags & DebugDraw.DrawFlags.Obb) != 0) + { + Color color = new Color(0.5f, 0.3f, 0.5f); + + for (Body b = _bodyList; b != null; b = b.GetNext()) + { + XForm xf = b.GetXForm(); + for (Shape s = b.GetShapeList(); s != null; s = s.GetNext()) + { + if (s.GetType() != ShapeType.PolygonShape) + { + continue; + } + + PolygonShape poly = (PolygonShape)s; + OBB obb = poly.GetOBB(); + Vec2 h = obb.Extents; + Vec2[] vs = new Vec2[4]; + vs[0].Set(-h.X, -h.Y); + vs[1].Set(h.X, -h.Y); + vs[2].Set(h.X, h.Y); + vs[3].Set(-h.X, h.Y); + + for (int i = 0; i < 4; ++i) + { + vs[i] = obb.Center + Common.Math.Mul(obb.R, vs[i]); + vs[i] = Common.Math.Mul(xf, vs[i]); + } + + _debugDraw.DrawPolygon(vs, 4, color); + } + } + } + if ((flags & DebugDraw.DrawFlags.CenterOfMass) != 0) { for (Body b = _bodyList; b != null; b = b.GetNext()) @@ -1420,17 +1399,17 @@ namespace Box2DX.Dynamics //Is it safe to pass private static function pointers? private static float RaycastSortKey(object data) { - Fixture fixture = data as Fixture; - Box2DXDebug.Assert(fixture != null); - Body body = fixture.Body; + Shape shape = data as Shape; + Box2DXDebug.Assert(shape != null); + Body body = shape.GetBody(); World world = body.GetWorld(); + XForm xf = body.GetXForm(); - if (world._contactFilter != null && !world._contactFilter.RayCollide(world._raycastUserData, fixture)) + if (world._contactFilter!=null && !world._contactFilter.RayCollide(world._raycastUserData, shape)) return -1; float lambda; - - SegmentCollide collide = fixture.TestSegment(out lambda, out world._raycastNormal, world._raycastSegment, 1); + SegmentCollide collide = shape.TestSegment(xf, out lambda, out world._raycastNormal, world._raycastSegment, 1); if (world._raycastSolidShape && collide == SegmentCollide.MissCollide) return -1; diff --git a/Box2d/Assets/Program/Box2d/Dynamics/WorldCallbacks.cs b/Box2d/Assets/Program/Box2d/Dynamics/WorldCallbacks.cs index 4f7909b..6bd7f35 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/WorldCallbacks.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/WorldCallbacks.cs @@ -1,6 +1,6 @@ /* - Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version 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 @@ -20,6 +20,8 @@ */ using System; +using System.Collections.Generic; +using System.Text; using Box2DX.Common; using Box2DX.Collision; @@ -43,7 +45,7 @@ namespace Box2DX.Dynamics /// Called when any shape is about to be destroyed due /// to the destruction of its parent body. /// - public abstract void SayGoodbye(Fixture fixture); + public abstract void SayGoodbye(Shape shape); } /// @@ -69,45 +71,42 @@ namespace Box2DX.Dynamics /// If you implement your own collision filter you may want to build from this implementation. /// @warning for performance reasons this is only called when the AABBs begin to overlap. /// - public virtual bool ShouldCollide(Fixture fixtureA, Fixture fixtureB) + public virtual bool ShouldCollide(Shape shape1, Shape shape2) { - FilterData filterA = fixtureA.Filter; - FilterData filterB = fixtureB.Filter; + FilterData filter1 = shape1.FilterData; + FilterData filter2 = shape2.FilterData; - if (filterA.GroupIndex == filterB.GroupIndex && filterA.GroupIndex != 0) + if (filter1.GroupIndex == filter2.GroupIndex && filter1.GroupIndex != 0) { - return filterA.GroupIndex > 0; + return filter1.GroupIndex > 0; } - bool collide = (filterA.MaskBits & filterB.CategoryBits) != 0 && (filterA.CategoryBits & filterB.MaskBits) != 0; + bool collide = (filter1.MaskBits & filter2.CategoryBits) != 0 && (filter1.CategoryBits & filter2.MaskBits) != 0; return collide; } /// /// Return true if the given shape should be considered for ray intersection. /// - public bool RayCollide(object userData, Fixture fixture) + public bool RayCollide(object userData, Shape shape) { //By default, cast userData as a shape, and then collide if the shapes would collide if (userData == null) - { return true; - } - - return ShouldCollide((Fixture)userData, fixture); + return ShouldCollide((Shape)userData, shape); } } - /// Contact impulses for reporting. Impulses are used instead of forces because - /// sub-step forces may approach infinity for rigid body collisions. These - /// match up one-to-one with the contact points in b2Manifold. - public class ContactImpulse + public class WorldCallback { - public float[] normalImpulses = new float[Settings.MaxManifoldPoints]; - public float[] tangentImpulses = new float[Settings.MaxManifoldPoints]; + /// + /// The default contact filter. + /// + public static ContactFilter DefaultFilter = new ContactFilter(); } - /// Implement this class to get contact information. You can use these results for + /// + /// Implement this class to get collision results. You can use these results for /// things like sounds and game logic. You can also get contact results by /// traversing the contact lists after the time step. However, you might miss /// some contacts because continuous physics leads to sub-stepping. @@ -115,34 +114,33 @@ namespace Box2DX.Dynamics /// single time step. /// You should strive to make your callbacks efficient because there may be /// many callbacks per time step. - /// @warning You cannot create/destroy Box2DX entities inside these callbacks. - public interface ContactListener + /// @warning The contact separation is the last computed value. + /// @warning You cannot create/destroy Box2D entities inside these callbacks. + /// + public abstract class ContactListener { - /// Called when two fixtures begin to touch. - void BeginContact(Contact contact); - - /// Called when two fixtures cease to touch. - void EndContact(Contact contact); - - /// This is called after a contact is updated. This allows you to inspect a - /// contact before it goes to the solver. If you are careful, you can modify the - /// contact manifold (e.g. disable contact). - /// A copy of the old manifold is provided so that you can detect changes. - /// Note: this is called only for awake bodies. - /// Note: this is called even when the number of contact points is zero. - /// Note: this is not called for sensors. - /// Note: if you set the number of contact points to zero, you will not - /// get an EndContact callback. However, you may get a BeginContact callback - /// the next step. - void PreSolve(Contact contact, Manifold oldManifold); - - /// This lets you inspect a contact after the solver is finished. This is useful - /// for inspecting impulses. - /// Note: the contact manifold does not include time of impact impulses, which can be - /// arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly - /// in a separate data structure. - /// Note: this is only called for contacts that are touching, solid, and awake. - void PostSolve(Contact contact, ContactImpulse impulse); + /// + /// Called when a contact point is added. This includes the geometry + /// and the forces. + /// + public virtual void Add(ContactPoint point) { return; } + + /// + /// Called when a contact point persists. This includes the geometry + /// and the forces. + /// + public virtual void Persist(ContactPoint point) { return; } + + /// + /// Called when a contact point is removed. This includes the last + /// computed geometry and forces. + /// + public virtual void Remove(ContactPoint point) { return; } + + /// + /// Called after a contact point is solved. + /// + public virtual void Result(ContactResult point) { return; } } /// @@ -156,10 +154,6 @@ namespace Box2DX.Dynamics { R = r; G = g; B = b; } - public void Set(float r, float g, float b) - { - R = r; G = g; B = b; - } } /// @@ -173,13 +167,12 @@ namespace Box2DX.Dynamics { Shape = 0x0001, // draw shapes Joint = 0x0002, // draw joint connections - CoreShape = 0x0004, // draw core (TOI) shapes // should be removed in this revision? + CoreShape = 0x0004, // draw core (TOI) shapes Aabb = 0x0008, // draw axis aligned bounding boxes - Obb = 0x0010, // draw oriented bounding boxes // should be removed in this revision? + Obb = 0x0010, // draw oriented bounding boxes Pair = 0x0020, // draw broad-phase pairs - CenterOfMass = 0x0040, // draw center of mass frame - Controller = 0x0080 // draw center of mass frame - }; + CenterOfMass = 0x0040 // draw center of mass frame + } protected DrawFlags _drawFlags; diff --git a/Box2d/Assets/Scenes/TestScene.unity b/Box2d/Assets/Scenes/TestScene.unity index 6752e17..155809d 100644 --- a/Box2d/Assets/Scenes/TestScene.unity +++ b/Box2d/Assets/Scenes/TestScene.unity @@ -53,12 +53,12 @@ LightmapSettings: m_TemporalCoherenceThreshold: 1 m_EnvironmentLightingMode: 0 m_EnableBakedLightmaps: 1 - m_EnableRealtimeLightmaps: 0 + m_EnableRealtimeLightmaps: 1 m_LightmapEditorSettings: serializedVersion: 10 m_Resolution: 2 - m_BakeResolution: 10 - m_AtlasSize: 512 + m_BakeResolution: 40 + m_AtlasSize: 1024 m_AO: 0 m_AOMaxDistance: 1 m_CompAOExponent: 1 @@ -75,7 +75,7 @@ LightmapSettings: m_BakeBackend: 1 m_PVRSampling: 1 m_PVRDirectSampleCount: 32 - m_PVRSampleCount: 256 + m_PVRSampleCount: 500 m_PVRBounces: 2 m_PVRFilterTypeDirect: 0 m_PVRFilterTypeIndirect: 0 @@ -113,15 +113,15 @@ NavMeshSettings: debug: m_Flags: 0 m_NavMeshData: {fileID: 0} ---- !u!1 &170076733 +--- !u!1 &258777548 GameObject: m_ObjectHideFlags: 0 m_CorrespondingSourceObject: {fileID: 0} m_PrefabInternal: {fileID: 0} serializedVersion: 6 m_Component: - - component: {fileID: 170076735} - - component: {fileID: 170076734} + - component: {fileID: 258777550} + - component: {fileID: 258777549} m_Layer: 0 m_Name: Directional Light m_TagString: Untagged @@ -129,12 +129,12 @@ GameObject: m_NavMeshLayer: 0 m_StaticEditorFlags: 0 m_IsActive: 1 ---- !u!108 &170076734 +--- !u!108 &258777549 Light: m_ObjectHideFlags: 0 m_CorrespondingSourceObject: {fileID: 0} m_PrefabInternal: {fileID: 0} - m_GameObject: {fileID: 170076733} + m_GameObject: {fileID: 258777548} m_Enabled: 1 serializedVersion: 8 m_Type: 1 @@ -158,7 +158,7 @@ Light: m_CullingMask: serializedVersion: 2 m_Bits: 4294967295 - m_Lightmapping: 1 + m_Lightmapping: 4 m_LightShadowCasterMode: 0 m_AreaSize: {x: 1, y: 1} m_BounceIntensity: 1 @@ -166,12 +166,12 @@ Light: m_UseColorTemperature: 0 m_ShadowRadius: 0 m_ShadowAngle: 0 ---- !u!4 &170076735 +--- !u!4 &258777550 Transform: m_ObjectHideFlags: 0 m_CorrespondingSourceObject: {fileID: 0} m_PrefabInternal: {fileID: 0} - m_GameObject: {fileID: 170076733} + m_GameObject: {fileID: 258777548} m_LocalRotation: {x: 0.40821788, y: -0.23456968, z: 0.10938163, w: 0.8754261} m_LocalPosition: {x: 0, y: 3, z: 0} m_LocalScale: {x: 1, y: 1, z: 1} @@ -179,40 +179,40 @@ Transform: m_Father: {fileID: 0} m_RootOrder: 1 m_LocalEulerAnglesHint: {x: 50, y: -30, z: 0} ---- !u!1 &534669902 +--- !u!1 &511688542 GameObject: m_ObjectHideFlags: 0 m_CorrespondingSourceObject: {fileID: 0} m_PrefabInternal: {fileID: 0} serializedVersion: 6 m_Component: - - component: {fileID: 534669905} - - component: {fileID: 534669904} - - component: {fileID: 534669903} + - component: {fileID: 511688545} + - component: {fileID: 511688544} + - component: {fileID: 511688543} m_Layer: 0 m_Name: Main Camera - m_TagString: Untagged + m_TagString: MainCamera m_Icon: {fileID: 0} m_NavMeshLayer: 0 m_StaticEditorFlags: 0 m_IsActive: 1 ---- !u!81 &534669903 +--- !u!81 &511688543 AudioListener: m_ObjectHideFlags: 0 m_CorrespondingSourceObject: {fileID: 0} m_PrefabInternal: {fileID: 0} - m_GameObject: {fileID: 534669902} - m_Enabled: 1 ---- !u!20 &534669904 + m_GameObject: {fileID: 511688542} + m_Enabled: 0 +--- !u!20 &511688544 Camera: m_ObjectHideFlags: 0 m_CorrespondingSourceObject: {fileID: 0} m_PrefabInternal: {fileID: 0} - m_GameObject: {fileID: 534669902} + m_GameObject: {fileID: 511688542} m_Enabled: 1 serializedVersion: 2 - m_ClearFlags: 1 - m_BackGroundColor: {r: 0.19215687, g: 0.3019608, b: 0.4745098, a: 0} + m_ClearFlags: 2 + m_BackGroundColor: {r: 0, g: 0, b: 0, a: 0} m_projectionMatrixMode: 1 m_SensorSize: {x: 36, y: 24} m_LensShift: {x: 0, y: 0} @@ -223,10 +223,10 @@ Camera: y: 0 width: 1 height: 1 - near clip plane: 0.3 - far clip plane: 1000 + near clip plane: 0 + far clip plane: 10 field of view: 60 - orthographic: 0 + orthographic: 1 orthographic size: 5 m_Depth: -1 m_CullingMask: @@ -243,56 +243,58 @@ Camera: m_OcclusionCulling: 1 m_StereoConvergence: 10 m_StereoSeparation: 0.022 ---- !u!4 &534669905 +--- !u!4 &511688545 Transform: m_ObjectHideFlags: 0 m_CorrespondingSourceObject: {fileID: 0} m_PrefabInternal: {fileID: 0} - m_GameObject: {fileID: 534669902} + m_GameObject: {fileID: 511688542} m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} - m_LocalPosition: {x: 0, y: 1, z: -10} + m_LocalPosition: {x: 0, y: 0, z: -1} m_LocalScale: {x: 1, y: 1, z: 1} m_Children: [] m_Father: {fileID: 0} m_RootOrder: 0 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} ---- !u!1 &1272347721 -GameObject: - m_ObjectHideFlags: 0 - m_CorrespondingSourceObject: {fileID: 0} - m_PrefabInternal: {fileID: 0} - serializedVersion: 6 - m_Component: - - component: {fileID: 1272347723} - - component: {fileID: 1272347722} - m_Layer: 0 - m_Name: Application - m_TagString: Untagged - m_Icon: {fileID: 0} - m_NavMeshLayer: 0 - m_StaticEditorFlags: 0 - m_IsActive: 1 ---- !u!114 &1272347722 -MonoBehaviour: +--- !u!1001 &1398880139 +Prefab: m_ObjectHideFlags: 0 - m_CorrespondingSourceObject: {fileID: 0} - m_PrefabInternal: {fileID: 0} - m_GameObject: {fileID: 1272347721} - m_Enabled: 1 - m_EditorHideFlags: 0 - m_Script: {fileID: 11500000, guid: 2dc4697d525f9454e9497e4fc4ac0a66, type: 3} - m_Name: - m_EditorClassIdentifier: ---- !u!4 &1272347723 -Transform: - m_ObjectHideFlags: 0 - m_CorrespondingSourceObject: {fileID: 0} - m_PrefabInternal: {fileID: 0} - m_GameObject: {fileID: 1272347721} - m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} - m_LocalPosition: {x: -34.56905, y: -8.307981, z: -0.65625} - m_LocalScale: {x: 1, y: 1, z: 1} - m_Children: [] - m_Father: {fileID: 0} - m_RootOrder: 2 - m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} + serializedVersion: 2 + m_Modification: + m_TransformParent: {fileID: 0} + m_Modifications: + - target: {fileID: 4861056112530586, guid: 34ebd6d57db1403459f3131db297b063, type: 2} + propertyPath: m_LocalPosition.x + value: -5.75 + objectReference: {fileID: 0} + - target: {fileID: 4861056112530586, guid: 34ebd6d57db1403459f3131db297b063, type: 2} + propertyPath: m_LocalPosition.y + value: 2.87 + objectReference: {fileID: 0} + - target: {fileID: 4861056112530586, guid: 34ebd6d57db1403459f3131db297b063, type: 2} + propertyPath: m_LocalPosition.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4861056112530586, guid: 34ebd6d57db1403459f3131db297b063, type: 2} + propertyPath: m_LocalRotation.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4861056112530586, guid: 34ebd6d57db1403459f3131db297b063, type: 2} + propertyPath: m_LocalRotation.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4861056112530586, guid: 34ebd6d57db1403459f3131db297b063, type: 2} + propertyPath: m_LocalRotation.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4861056112530586, guid: 34ebd6d57db1403459f3131db297b063, type: 2} + propertyPath: m_LocalRotation.w + value: 1 + objectReference: {fileID: 0} + - target: {fileID: 4861056112530586, guid: 34ebd6d57db1403459f3131db297b063, type: 2} + propertyPath: m_RootOrder + value: 2 + objectReference: {fileID: 0} + m_RemovedComponents: [] + m_SourcePrefab: {fileID: 100100000, guid: 34ebd6d57db1403459f3131db297b063, type: 2} + m_IsPrefabAsset: 0 diff --git a/Box2d/Assets/smcs.rsp b/Box2d/Assets/smcs.rsp deleted file mode 100644 index e96009c..0000000 --- a/Box2d/Assets/smcs.rsp +++ /dev/null @@ -1 +0,0 @@ --unsafe -warnaserror- \ No newline at end of file diff --git a/Box2d/Assets/smcs.rsp.meta b/Box2d/Assets/smcs.rsp.meta deleted file mode 100644 index 5738f9b..0000000 --- a/Box2d/Assets/smcs.rsp.meta +++ /dev/null @@ -1,7 +0,0 @@ -fileFormatVersion: 2 -guid: f9de2fde42ec7ec46af4b5935718a1aa -DefaultImporter: - externalObjects: {} - userData: - assetBundleName: - assetBundleVariant: -- cgit v1.1-26-g67d0