/// <summary> /// Returns the index of the axis with the max circle penetration depth. /// Breaks out if a separating axis is found between the two shapes. /// Outputs the penetration depth of the circle in the axis (if any). /// </summary> internal static int FindAxisMaxPenetration( TSVector2 origin, FP radius, VoltPolygon poly, out FP penetration) { int index = 0; int found = 0; penetration = FP.NegativeInfinity; for (int i = 0; i < poly.countWorld; i++) { Axis axis = poly.worldAxes[i]; FP dot = TSVector2.Dot(axis.Normal, origin); FP dist = dot - axis.Width - radius; if (dist > 0) { return(-1); } if (dist > penetration) { penetration = dist; found = index; } index++; } return(found); }
private static bool FindMinSepAxis( VoltPolygon poly1, VoltPolygon poly2, out Axis axis) { axis = new Axis(TSVector2.zero, FP.NegativeInfinity); for (int i = 0; i < poly1.countWorld; i++) { Axis a = poly1.worldAxes[i]; FP min = FP.PositiveInfinity; for (int j = 0; j < poly2.countWorld; j++) { TSVector2 v = poly2.worldVertices[j]; min = TSMath.Min(min, TSVector2.Dot(a.Normal, v)); } min -= a.Width; if (min > 0) { return(false); } if (min > axis.Width) { axis = new Axis(a.Normal, min); } } return(true); }
/// <summary> /// Returns the index of the nearest axis on the poly to a point. /// Outputs the minimum distance between the axis and the point. /// </summary> internal static int FindAxisShortestDistance( TSVector2 point, Axis[] axes, out FP minDistance) { int ix = 0; minDistance = FP.PositiveInfinity; bool inside = true; for (int i = 0; i < axes.Length; i++) { FP dot = TSVector2.Dot(axes[i].Normal, point); FP dist = axes[i].Width - dot; if (dist < 0.0f) { inside = false; } if (dist < minDistance) { minDistance = dist; ix = i; } } if (inside == true) { minDistance = 0.0f; ix = -1; } return(ix); }
internal Axis BodyToWorldAxis(Axis axis) { TSVector2 normal = axis.Normal.Rotate(this.facing); FP width = TSVector2.Dot(normal, this.position) + axis.Width; return(new Axis(normal, width)); }
/// <summary> /// A world-space point query, used as a shortcut in collision tests. /// </summary> internal bool ContainsPoint( TSVector2 worldSpacePoint) { for (int i = 0; i < this.countWorld; i++) { Axis axis = this.worldAxes[i]; if (TSVector2.Dot(axis.Normal, worldSpacePoint) > axis.Width) { return(false); } } return(true); }
protected override bool ShapeQueryPoint( TSVector2 bodySpacePoint) { for (int i = 0; i < this.countBody; i++) { Axis axis = this.bodyAxes[i]; if (TSVector2.Dot(axis.Normal, bodySpacePoint) > axis.Width) { return(false); } } return(true); }
/// <summary> /// Special case that ignores axes pointing away from the normal. /// </summary> internal bool ContainsPointPartial( TSVector2 worldSpacePoint, TSVector2 worldSpaceNormal) { foreach (Axis axis in this.worldAxes) { if (TSVector2.Dot(axis.Normal, worldSpaceNormal) >= 0.0f && TSVector2.Dot(axis.Normal, worldSpacePoint) > axis.Width) { return(false); } } return(true); }
private static void ComputeAxes( TSVector2[] vertices, int count, ref Axis[] destination) { if (destination.Length < count) { destination = new Axis[count]; } for (int i = 0; i < count; i++) { TSVector2 u = vertices[i]; TSVector2 v = vertices[(i + 1) % count]; TSVector2 normal = (v - u).Left().normalized; destination[i] = new Axis(normal, TSVector2.Dot(normal, u)); } }
private FP ComputeInertia() { FP s1 = 0.0f; FP s2 = 0.0f; for (int i = 0; i < this.countBody; i++) { TSVector2 v = this.bodyVertices[i]; TSVector2 u = this.bodyVertices[(i + 1) % this.countBody]; FP a = VoltMath.Cross(u, v); FP b = v.sqrMagnitude + u.sqrMagnitude + TSVector2.Dot(v, u); s1 += a * b; s2 += a; } return(s1 / (6.0f * s2)); }
/// <summary> /// Checks a ray against a circle with a given origin and square radius. /// </summary> internal static bool CircleRayCast( VoltShape shape, TSVector2 shapeOrigin, FP sqrRadius, ref VoltRayCast ray, ref VoltRayResult result) { TSVector2 toOrigin = shapeOrigin - ray.origin; if (toOrigin.sqrMagnitude < sqrRadius) { result.SetContained(shape); return(true); } FP slope = TSVector2.Dot(toOrigin, ray.direction); if (slope < 0) { return(false); } FP sqrSlope = slope * slope; FP d = sqrRadius + sqrSlope - TSVector2.Dot(toOrigin, toOrigin); if (d < 0) { return(false); } FP dist = slope - TSMath.Sqrt(d); if (dist < 0 || dist > ray.distance) { return(false); } // N.B.: For historical raycasts this normal will be wrong! // Must be either transformed back to world or invalidated later. TSVector2 normal = (dist * ray.direction - toOrigin).normalized; result.Set(shape, dist, normal); return(true); }
internal void Solve(Manifold manifold) { VoltBody bodyA = manifold.ShapeA.Body; VoltBody bodyB = manifold.ShapeB.Body; FP elasticity = bodyA.World.Elasticity; // Calculate relative bias velocity TSVector2 vb1 = bodyA.BiasVelocity + (bodyA.BiasRotation * this.toALeft); TSVector2 vb2 = bodyB.BiasVelocity + (bodyB.BiasRotation * this.toBLeft); FP vbn = TSVector2.Dot((vb1 - vb2), this.normal); // Calculate and clamp the bias impulse FP jbn = this.nMass * (vbn - this.bias); jbn = TSMath.Max(-this.jBias, jbn); this.jBias += jbn; // Apply the bias impulse this.ApplyNormalBiasImpulse(bodyA, bodyB, jbn); // Calculate relative velocity TSVector2 vr = this.RelativeVelocity(bodyA, bodyB); FP vrn = TSVector2.Dot(vr, this.normal); // Calculate and clamp the normal impulse FP jn = nMass * (vrn + (this.restitution * elasticity)); jn = TSMath.Max(-this.cachedNormalImpulse, jn); this.cachedNormalImpulse += jn; // Calculate the relative tangent velocity FP vrt = TSVector2.Dot(vr, this.normal.Left()); // Calculate and clamp the friction impulse FP jtMax = manifold.Friction * this.cachedNormalImpulse; FP jt = vrt * tMass; FP result = TSMath.Clamp(this.cachedTangentImpulse + jt, -jtMax, jtMax); jt = result - this.cachedTangentImpulse; this.cachedTangentImpulse = result; // Apply the normal and tangent impulse this.ApplyContactImpulse(bodyA, bodyB, jn, jt); }
internal void PreStep(Manifold manifold) { VoltBody bodyA = manifold.ShapeA.Body; VoltBody bodyB = manifold.ShapeB.Body; this.toA = this.position - bodyA.Position; this.toB = this.position - bodyB.Position; this.toALeft = this.toA.Left(); this.toBLeft = this.toB.Left(); this.nMass = 1.0f / this.KScalar(bodyA, bodyB, this.normal); this.tMass = 1.0f / this.KScalar(bodyA, bodyB, this.normal.Left()); this.bias = Contact.BiasDist(penetration); this.jBias = 0; this.restitution = manifold.Restitution * TSVector2.Dot( this.normal, this.RelativeVelocity(bodyA, bodyB)); }
private bool CircleCastEdges( ref VoltRayCast bodySpaceRay, FP radius, ref VoltRayResult result) { int foundIndex = -1; bool couldBeContained = true; // Pre-compute and initialize values FP shortestDist = FP.MaxValue; TSVector2 v3 = bodySpaceRay.direction.Left(); // Check the edges -- this will be different from the raycast because // we care about staying within the ends of the edge line segment for (int i = 0; i < this.countBody; i++) { Axis curAxis = this.bodyAxes[i]; // Push the edges out by the radius TSVector2 extension = curAxis.Normal * radius; TSVector2 a = this.bodyVertices[i] + extension; TSVector2 b = this.bodyVertices[(i + 1) % this.countBody] + extension; // Update the check for containment if (couldBeContained == true) { FP proj = TSVector2.Dot(curAxis.Normal, bodySpaceRay.origin) - curAxis.Width; // The point lies outside of the outer layer if (proj > radius) { couldBeContained = false; } // The point lies between the outer and inner layer else if (proj > 0.0f) { // See if the point is within the center Vornoi region of the edge FP d = VoltMath.Cross(curAxis.Normal, bodySpaceRay.origin); if (d > VoltMath.Cross(curAxis.Normal, a)) { couldBeContained = false; } if (d < VoltMath.Cross(curAxis.Normal, b)) { couldBeContained = false; } } } // For the cast, only consider rays pointing towards the edge if (TSVector2.Dot(curAxis.Normal, bodySpaceRay.direction) >= 0.0f) { continue; } // See: // https://rootllama.wordpress.com/2014/06/20/ray-line-segment-intersection-test-in-2d/ TSVector2 v1 = bodySpaceRay.origin - a; TSVector2 v2 = b - a; FP denominator = TSVector2.Dot(v2, v3); FP t1 = VoltMath.Cross(v2, v1) / denominator; FP t2 = TSVector2.Dot(v1, v3) / denominator; if ((t2 >= 0.0f) && (t2 <= 1.0f) && (t1 > 0.0f) && (t1 < shortestDist)) { // See if the point is outside of any of the axes shortestDist = t1; foundIndex = i; } } // Report results if (couldBeContained == true) { result.SetContained(this); return(true); } else if (foundIndex >= 0 && shortestDist <= bodySpaceRay.distance) { result.Set( this, shortestDist, this.bodyAxes[foundIndex].Normal); return(true); } return(false); }
protected override bool ShapeRayCast( ref VoltRayCast bodySpaceRay, ref VoltRayResult result) { int foundIndex = -1; FP inner = FP.MaxValue; FP outer = 0; bool couldBeContained = true; for (int i = 0; i < this.countBody; i++) { Axis curAxis = this.bodyAxes[i]; // Distance between the ray origin and the axis/edge along the // normal (i.e., shortest distance between ray origin and the edge) FP proj = TSVector2.Dot(curAxis.Normal, bodySpaceRay.origin) - curAxis.Width; // See if the point is outside of any of the axes if (proj > 0.0f) { couldBeContained = false; } // Projection of the ray direction onto the axis normal (use // negative normal because we want to get the penetration length) FP slope = TSVector2.Dot(-curAxis.Normal, bodySpaceRay.direction); if (slope == 0.0f) { continue; } FP dist = proj / slope; // The ray is pointing opposite the edge normal (towards the edge) if (slope > 0.0f) { if (dist > inner) { return(false); } if (dist > outer) { outer = dist; foundIndex = i; } } // The ray is pointing along the edge normal (away from the edge) else { if (dist < outer) { return(false); } if (dist < inner) { inner = dist; } } } if (couldBeContained == true) { result.SetContained(this); return(true); } else if (foundIndex >= 0 && outer <= bodySpaceRay.distance) { result.Set( this, outer, this.bodyAxes[foundIndex].Normal); return(true); } return(false); }