diff options
Diffstat (limited to 'Box2d/Assets/Program/Box2d/Collision/Shapes')
7 files changed, 1353 insertions, 3 deletions
diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs b/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs new file mode 100644 index 0000000..553accd --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs @@ -0,0 +1,179 @@ +/* + 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 +{ + /// <summary> + /// A circle shape. + /// </summary> + public class CircleShape : Shape + { + // Position + internal Vec2 _position; + + public CircleShape() + { + _type = ShapeType.CircleShape; + } + + public override bool TestPoint(XForm transform, Vec2 p) + { + Vec2 center = transform.Position + Common.Math.Mul(transform.R, _position); + Vec2 d = p - center; + return Vec2.Dot(d, d) <= _radius * _radius; + } + + // Collision Detection in Interactive 3D Environments by Gino van den Bergen + // From Section 3.1.2 + // x = s + a * r + // norm(x) = radius + public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda) + { + lambda = 0f; + normal = Vec2.Zero; + + Vec2 position = transform.Position + Common.Math.Mul(transform.R, _position); + Vec2 s = segment.P1 - position; + float b = Vec2.Dot(s, s) - _radius * _radius; + + // Does the segment start inside the circle? + if (b < 0.0f) + { + lambda = 0f; + return SegmentCollide.StartInsideCollide; + } + + // Solve quadratic equation. + Vec2 r = segment.P2 - segment.P1; + float c = Vec2.Dot(s, r); + float rr = Vec2.Dot(r, r); + float sigma = c * c - rr * b; + + // Check for negative discriminant and short segment. + if (sigma < 0.0f || rr < Common.Settings.FLT_EPSILON) + { + return SegmentCollide.MissCollide; + } + + // Find the point of intersection of the line with the circle. + float a = -(c + Common.Math.Sqrt(sigma)); + + // Is the intersection point on the segment? + if (0.0f <= a && a <= maxLambda * rr) + { + a /= rr; + lambda = a; + normal = s + a * r; + normal.Normalize(); + return SegmentCollide.HitCollide; + } + + return SegmentCollide.MissCollide; + } + + public override void ComputeAABB(out AABB aabb, XForm transform) + { + aabb = new AABB(); + + Vec2 p = transform.Position + Common.Math.Mul(transform.R, _position); + 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) + { + massData = new MassData(); + + massData.Mass = density * Settings.Pi * _radius * _radius; + massData.Center = _position; + + // inertia about the local origin + massData.I = massData.Mass * (0.5f * _radius * _radius + Vec2.Dot(_position, _position)); + } + + public override float ComputeSubmergedArea(Vec2 normal, float offset, XForm xf, out Vec2 c) + { + 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; + + return area; + } + + /// <summary> + /// Get the supporting vertex index in the given direction. + /// </summary> + public override int GetSupport(Vec2 d) + { + return 0; + } + + /// <summary> + /// Get the supporting vertex in the given direction. + /// </summary> + public override Vec2 GetSupportVertex(Vec2 d) + { + return _position; + } + + /// <summary> + /// Get a vertex by index. Used by Distance. + /// </summary> + public override Vec2 GetVertex(int index) + { + Box2DXDebug.Assert(index == 0); + return _position; + } + + public override float ComputeSweepRadius(Vec2 pivot) + { + return Vec2.Distance(_position, pivot); + } + + /// <summary> + /// Get the vertex count. + /// </summary> + public int VertexCount { get { return 1; } } + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs.meta b/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs.meta new file mode 100644 index 0000000..f9be13f --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: bd3059081b10a7c49be772d675ea7525 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs b/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs new file mode 100644 index 0000000..348a293 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs @@ -0,0 +1,277 @@ +/* + 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 new file mode 100644 index 0000000..8051fd4 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs.meta @@ -0,0 +1,11 @@ +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 85ae672..6a655f7 100644 --- a/Box2d/Assets/Program/Box2d/Collision/Shapes/PolygonShape.cs +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/PolygonShape.cs @@ -1,7 +1,724 @@ -using System.Collections; +/* + 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. +*/ + +#define DEBUG + +using System; using System.Collections.Generic; +using System.Text; + +using Box2DX.Common; -namespace Box2D +namespace Box2DX.Collision { + /// <summary> + /// A convex polygon. It is assumed that the interior of the polygon is to the left of each edge. + /// </summary> + public class PolygonShape : Shape + { + internal Vec2 _centroid; + internal Vec2[] _vertices = new Vec2[Settings.MaxPolygonVertices]; + internal Vec2[] _normals = new Vec2[Settings.MaxPolygonVertices]; + + internal int _vertexCount; + + public int VertexCount + { + get { return _vertexCount; } + } + + public Vec2[] Vertices + { + get { return _vertices; } + } + + /// <summary> + /// Copy vertices. This assumes the vertices define a convex polygon. + /// It is assumed that the exterior is the the right of each edge. + /// </summary> + public void Set(Vec2[] vertices, int count) + { + Box2DXDebug.Assert(3 <= count && count <= Settings.MaxPolygonVertices); + _vertexCount = count; + + int i; + // Copy vertices. + for (i = 0; i < _vertexCount; ++i) + { + _vertices[i] = vertices[i]; + } + + // Compute normals. Ensure the edges have non-zero length. + for (i = 0; i < _vertexCount; ++i) + { + int i1 = i; + int i2 = i + 1 < count ? i + 1 : 0; + Vec2 edge = _vertices[i2] - _vertices[i1]; + Box2DXDebug.Assert(edge.LengthSquared() > Settings.FLT_EPSILON_SQUARED); + _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) + { + 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) + { + 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); + } + } +#endif + + // Compute the polygon centroid. + _centroid = ComputeCentroid(_vertices, _vertexCount); + } + + /// <summary> + /// Build vertices to represent an axis-aligned box. + /// </summary> + /// <param name="hx">The half-width</param> + /// <param name="hy">The half-height.</param> + 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); + } + + + /// <summary> + /// Build vertices to represent an oriented box. + /// </summary> + /// <param name="hx">The half-width</param> + /// <param name="hy">The half-height.</param> + /// <param name="center">The center of the box in local coordinates.</param> + /// <param name="angle">The rotation of the box in local coordinates.</param> + 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); + + // Transform vertices and normals. + for (int i = 0; i < _vertexCount; ++i) + { + _vertices[i] = Common.Math.Mul(xf, _vertices[i]); + _normals[i] = Common.Math.Mul(xf.R, _normals[i]); + } + } + + public void SetAsEdge(Vec2 v1, Vec2 v2) + { + _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]; + } + + 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) + { + float dot = Vec2.Dot(_normals[i], pLocal - _vertices[i]); + if (dot > 0.0f) + { + return false; + } + } + + return true; + } + + public override SegmentCollide TestSegment(XForm xf, out float lambda, out Vec2 normal, Segment segment, float maxLambda) + { + lambda = 0f; + normal = Vec2.Zero; + + float lower = 0.0f, upper = maxLambda; + + Vec2 p1 = Common.Math.MulT(xf.R, segment.P1 - xf.Position); + Vec2 p2 = Common.Math.MulT(xf.R, segment.P2 - xf.Position); + Vec2 d = p2 - p1; + int index = -1; + + for (int i = 0; i < _vertexCount; ++i) + { + // p = p1 + a * d + // dot(normal, p - v) = 0 + // dot(normal, p1 - v) + a * dot(normal, d) = 0 + float numerator = Vec2.Dot(_normals[i], _vertices[i] - p1); + float denominator = Vec2.Dot(_normals[i], d); + + if (denominator == 0.0f) + { + if (numerator < 0.0f) + { + return SegmentCollide.MissCollide; + } + } + else + { + // Note: we want this predicate without division: + // lower < numerator / denominator, where denominator < 0 + // Since denominator < 0, we have to flip the inequality: + // lower < numerator / denominator <==> denominator * lower > numerator. + if (denominator < 0.0f && numerator < lower * denominator) + { + // Increase lower. + // The segment enters this half-space. + lower = numerator / denominator; + index = i; + } + else if (denominator > 0.0f && numerator < upper * denominator) + { + // Decrease upper. + // The segment exits this half-space. + upper = numerator / denominator; + } + } + + if (upper < lower) + { + return SegmentCollide.MissCollide; + } + } + + Box2DXDebug.Assert(0.0f <= lower && lower <= maxLambda); + + if (index >= 0) + { + lambda = lower; + normal = Common.Math.Mul(xf.R, _normals[index]); + return SegmentCollide.HitCollide; + } + + lambda = 0f; + return SegmentCollide.StartInsideCollide; + } + + 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); + } + + Vec2 r = new Vec2(_radius); + aabb.LowerBound = lower - r; + aabb.UpperBound = upper + r; + } + + public override void ComputeMass(out MassData massData, float denstity) + { + // Polygon mass, centroid, and inertia. + // Let rho be the polygon density in mass per unit area. + // Then: + // mass = rho * int(dA) + // centroid.x = (1/mass) * rho * int(x * dA) + // centroid.y = (1/mass) * rho * int(y * dA) + // I = rho * int((x*x + y*y) * dA) + // + // We can compute these integrals by summing all the integrals + // for each triangle of the polygon. To evaluate the integral + // for a single triangle, we make a change of variables to + // the (u,v) coordinates of the triangle: + // x = x0 + e1x * u + e2x * v + // y = y0 + e1y * u + e2y * v + // where 0 <= u && 0 <= v && u + v <= 1. + // + // We integrate u from [0,1-v] and then v from [0,1]. + // We also need to use the Jacobian of the transformation: + // D = cross(e1, e2) + // + // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) + // + // The rest of the derivation is handled by computer algebra. + + Box2DXDebug.Assert(_vertexCount >= 3); + + Vec2 center = new Vec2(0); + 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); + +#if O + // This code would put the reference point inside the polygon. + for (int i = 0; i < vCount; ++i) + { + pRef += _vertices[i]; + } + pRef *= 1.0f / count; +#endif + + const float k_inv3 = 1.0f / 3.0f; + + for (int i = 0; i < _vertexCount; ++i) + { + // Triangle vertices. + Vec2 p1 = pRef; + Vec2 p2 = _vertices[i]; + Vec2 p3 = i + 1 < _vertexCount ? _vertices[i + 1] : _vertices[0]; + + Vec2 e1 = p2 - p1; + Vec2 e2 = p3 - p1; + + float D = Vec2.Cross(e1, e2); + + float triangleArea = 0.5f * D; + area += triangleArea; + + // Area weighted centroid + center += triangleArea * k_inv3 * (p1 + p2 + p3); + + float px = p1.X, py = p1.Y; + float ex1 = e1.X, ey1 = e1.Y; + float ex2 = e2.X, ey2 = e2.Y; + + float intx2 = k_inv3 * (0.25f * (ex1 * ex1 + ex2 * ex1 + ex2 * ex2) + (px * ex1 + px * ex2)) + 0.5f * px * px; + float inty2 = k_inv3 * (0.25f * (ey1 * ey1 + ey2 * ey1 + ey2 * ey2) + (py * ey1 + py * ey2)) + 0.5f * py * py; + + I += D * (intx2 + inty2); + } + + // Total mass + massData.Mass = denstity * area; + + // Center of mass + Box2DXDebug.Assert(area > Common.Settings.FLT_EPSILON); + center *= 1.0f / area; + 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); + } + + /// <summary> + /// Get the supporting vertex index in the given direction. + /// </summary> + 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]; + } + + public static Vec2 ComputeCentroid(Vec2[] vs, int count) + { + Box2DXDebug.Assert(count >= 3); + + Vec2 c = new Vec2(0f); + float area = 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); +#if O + // This code would put the reference point inside the polygon. + for (int i = 0; i < count; ++i) + { + pRef += vs[i]; + } + pRef *= 1.0f / count; +#endif + + const float inv3 = 1.0f / 3.0f; + + for (int i = 0; i < count; ++i) + { + // Triangle vertices. + Vec2 p1 = pRef; + Vec2 p2 = vs[i]; + Vec2 p3 = i + 1 < count ? vs[i + 1] : vs[0]; + + Vec2 e1 = p2 - p1; + Vec2 e2 = p3 - p1; + + float D = Vec2.Cross(e1, e2); + + float triangleArea = 0.5f * D; + area += triangleArea; + + // Area weighted centroid + c += triangleArea * inv3 * (p1 + p2 + p3); + } + + // Centroid + Box2DXDebug.Assert(area > Common.Settings.FLT_EPSILON); + c *= 1.0f / area; + 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 + public static void ComputeOBB(out OBB obb, Vec2[] vs, int count) + { + obb = new OBB(); + + Box2DXDebug.Assert(count <= Settings.MaxPolygonVertices); + Vec2[] p = new Vec2[Settings.MaxPolygonVertices + 1]; + for (int i = 0; i < count; ++i) + { + p[i] = vs[i]; + } + p[count] = p[0]; + + float minArea = Common.Settings.FLT_MAX; + + for (int i = 1; i <= count; ++i) + { + Vec2 root = p[i - 1]; + Vec2 ux = p[i] - root; + float length = ux.Normalize(); + Box2DXDebug.Assert(length > Common.Settings.FLT_EPSILON); + Vec2 uy = new Vec2(-ux.Y, ux.X); + Vec2 lower = new Vec2(Common.Settings.FLT_MAX, Common.Settings.FLT_MAX); + Vec2 upper = new Vec2(-Common.Settings.FLT_MAX, -Common.Settings.FLT_MAX); + + for (int j = 0; j < count; ++j) + { + Vec2 d = p[j] - root; + Vec2 r = new Vec2(); + r.X = Vec2.Dot(ux, d); + r.Y = Vec2.Dot(uy, d); + lower = Common.Math.Min(lower, r); + upper = Common.Math.Max(upper, r); + } + + float area = (upper.X - lower.X) * (upper.Y - lower.Y); + if (area < 0.95f * minArea) + { + minArea = area; + obb.R.Col1 = ux; + obb.R.Col2 = uy; + Vec2 center = 0.5f * (lower + upper); + obb.Center = root + Common.Math.Mul(obb.R, center); + obb.Extents = 0.5f * (upper - lower); + } + } -} + 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 new file mode 100644 index 0000000..1afd116 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs @@ -0,0 +1,144 @@ +/* + 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 Box2DX.Common; + +namespace Box2DX.Collision +{ + /// <summary> + /// This holds the mass data computed for a shape. + /// </summary> + public struct MassData + { + /// <summary> + /// The mass of the shape, usually in kilograms. + /// </summary> + public float Mass; + + /// <summary> + /// The position of the shape's centroid relative to the shape's origin. + /// </summary> + public Vec2 Center; + + /// <summary> + /// The rotational inertia of the shape. + /// </summary> + public float I; + } + + /// <summary> + /// The various collision shape types supported by Box2D. + /// </summary> + public enum ShapeType + { + UnknownShape = -1, + CircleShape, + PolygonShape, + EdgeShape, + ShapeTypeCount, + } + + /// <summary> + /// Returns code from TestSegment + /// </summary> + public enum SegmentCollide + { + StartInsideCollide = -1, + MissCollide = 0, + HitCollide = 1 + } + + /// <summary> + /// 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. + /// </summary> + public abstract class Shape : IDisposable + { + #region Fields + + protected ShapeType _type = ShapeType.UnknownShape; + internal float _radius; + + #endregion Fields + + protected Shape() { } + + /// <summary> + /// Test a point for containment in this shape. This only works for convex shapes. + /// </summary> + /// <param name="xf">The shape world transform.</param> + /// <param name="p">A point in world coordinates.</param> + /// <returns></returns> + public abstract bool TestPoint(XForm xf, Vec2 p); + + /// <summary> + /// Perform a ray cast against this shape. + /// </summary> + /// <param name="xf">The shape world transform.</param> + /// <param name="lambda">Returns the hit fraction. You can use this to compute the contact point + /// p = (1 - lambda) * segment.P1 + lambda * segment.P2.</param> + /// <param name="normal"> Returns the normal at the contact point. If there is no intersection, + /// the normal is not set.</param> + /// <param name="segment">Defines the begin and end point of the ray cast.</param> + /// <param name="maxLambda">A number typically in the range [0,1].</param> + public abstract SegmentCollide TestSegment(XForm xf, out float lambda, out Vec2 normal, Segment segment, float maxLambda); + + /// <summary> + /// Given a transform, compute the associated axis aligned bounding box for this shape. + /// </summary> + /// <param name="aabb">Returns the axis aligned box.</param> + /// <param name="xf">The world transform of the shape.</param> + public abstract void ComputeAABB(out AABB aabb, XForm xf); + + /// <summary> + /// Compute the mass properties of this shape using its dimensions and density. + /// The inertia tensor is computed about the local origin, not the centroid. + /// </summary> + /// <param name="massData">Returns the mass data for this shape</param> + public abstract void ComputeMass(out MassData massData, float density); + + /// <summary> + /// Compute the volume and centroid of this shape intersected with a half plane. + /// </summary> + /// <param name="normal">Normal the surface normal.</param> + /// <param name="offset">Offset the surface offset along normal.</param> + /// <param name="xf">The shape transform.</param> + /// <param name="c">Returns the centroid.</param> + /// <returns>The total volume less than offset along normal.</returns> + public abstract float ComputeSubmergedArea(Vec2 normal, float offset, XForm xf, out Vec2 c); + + /// <summary> + /// Compute the sweep radius. This is used for conservative advancement (continuous collision detection). + /// </summary> + /// <param name="pivot">Pivot is the pivot point for rotation.</param> + /// <returns>The distance of the furthest point from the pivot.</returns> + public abstract float ComputeSweepRadius(Vec2 pivot); + + public abstract Vec2 GetVertex(int index); + + public abstract int GetSupport(Vec2 d); + + public abstract Vec2 GetSupportVertex(Vec2 d); + + public virtual void Dispose(){} + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs.meta b/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs.meta new file mode 100644 index 0000000..b67f3b4 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: cfde5d8f3b66ef849b75c54fff38f0b6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: |