private static bool FindMinSepAxis( VoltPolygon poly1, VoltPolygon poly2, out Axis axis) { axis = new Axis(VoltVector2.zero, Fix64.MinValue); for (int i = 0; i < poly1.countWorld; i++) { Axis a = poly1.worldAxes[i]; Fix64 min = Fix64.MaxValue; for (int j = 0; j < poly2.countWorld; j++) { VoltVector2 v = poly2.worldVertices[j]; min = VoltMath.Min(min, VoltVector2.Dot(a.Normal, v)); } min -= a.Width; if (min > Fix64.Zero) { return(false); } if (min > axis.Width) { axis = new Axis(a.Normal, min); } } return(true); }
/// <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( VoltVector2 origin, Fix64 radius, VoltPolygon poly, out Fix64 penetration) { int index = 0; int found = 0; penetration = Fix64.MinValue; for (int i = 0; i < poly.countWorld; i++) { Axis axis = poly.worldAxes[i]; Fix64 dot = VoltVector2.Dot(axis.Normal, origin); Fix64 dist = dot - axis.Width - radius; if (dist > Fix64.Zero) { return(-1); } if (dist > penetration) { penetration = dist; found = index; } index++; } return(found); }
/// <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( VoltVector2 point, Axis[] axes, out Fix64 minDistance) { int ix = 0; minDistance = Fix64.MaxValue; bool inside = true; for (int i = 0; i < axes.Length; i++) { Fix64 dot = VoltVector2.Dot(axes[i].Normal, point); Fix64 dist = axes[i].Width - dot; if (dist < Fix64.Zero) { inside = false; } if (dist < minDistance) { minDistance = dist; ix = i; } } if (inside == true) { minDistance = Fix64.Zero; ix = -1; } return(ix); }
internal Axis BodyToWorldAxis(Axis axis) { VoltVector2 normal = axis.Normal.Rotate(this.facing); Fix64 width = VoltVector2.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( VoltVector2 worldSpacePoint) { for (int i = 0; i < this.countWorld; i++) { Axis axis = this.worldAxes[i]; if (VoltVector2.Dot(axis.Normal, worldSpacePoint) > axis.Width) { return(false); } } return(true); }
protected override bool ShapeQueryPoint( VoltVector2 bodySpacePoint) { for (int i = 0; i < this.countBody; i++) { Axis axis = this.bodyAxes[i]; if (VoltVector2.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( VoltVector2 worldSpacePoint, VoltVector2 worldSpaceNormal) { foreach (Axis axis in this.worldAxes) { if (VoltVector2.Dot(axis.Normal, worldSpaceNormal) >= Fix64.Zero && VoltVector2.Dot(axis.Normal, worldSpacePoint) > axis.Width) { return(false); } } return(true); }
private static void ComputeAxes( VoltVector2[] vertices, int count, ref Axis[] destination) { if (destination.Length < count) { destination = new Axis[count]; } for (int i = 0; i < count; i++) { VoltVector2 u = vertices[i]; VoltVector2 v = vertices[(i + 1) % count]; VoltVector2 normal = (v - u).Left().normalized; destination[i] = new Axis(normal, VoltVector2.Dot(normal, u)); } }
private Fix64 ComputeInertia() { Fix64 s1 = Fix64.Zero; Fix64 s2 = Fix64.Zero; for (int i = 0; i < this.countBody; i++) { VoltVector2 v = this.bodyVertices[i]; VoltVector2 u = this.bodyVertices[(i + 1) % this.countBody]; Fix64 a = VoltMath.Cross(u, v); Fix64 b = v.sqrMagnitude + u.sqrMagnitude + VoltVector2.Dot(v, u); s1 += a * b; s2 += a; } return(s1 / ((Fix64)6 * s2)); }
internal void Solve(Manifold manifold) { VoltBody bodyA = manifold.ShapeA.Body; VoltBody bodyB = manifold.ShapeB.Body; Fix64 elasticity = bodyA.World.Elasticity; // Calculate relative bias velocity VoltVector2 vb1 = bodyA.BiasVelocity + (bodyA.BiasRotation * this.toALeft); VoltVector2 vb2 = bodyB.BiasVelocity + (bodyB.BiasRotation * this.toBLeft); Fix64 vbn = VoltVector2.Dot((vb1 - vb2), this.normal); // Calculate and clamp the bias impulse Fix64 jbn = this.nMass * (vbn - this.bias); jbn = VoltMath.Max(-this.jBias, jbn); this.jBias += jbn; // Apply the bias impulse this.ApplyNormalBiasImpulse(bodyA, bodyB, jbn); // Calculate relative velocity VoltVector2 vr = this.RelativeVelocity(bodyA, bodyB); Fix64 vrn = VoltVector2.Dot(vr, this.normal); // Calculate and clamp the normal impulse Fix64 jn = nMass * (vrn + (this.restitution * elasticity)); jn = VoltMath.Max(-this.cachedNormalImpulse, jn); this.cachedNormalImpulse += jn; // Calculate the relative tangent velocity Fix64 vrt = VoltVector2.Dot(vr, this.normal.Left()); // Calculate and clamp the friction impulse Fix64 jtMax = manifold.Friction * this.cachedNormalImpulse; Fix64 jt = vrt * tMass; Fix64 result = VoltMath.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); }
/// <summary> /// Checks a ray against a circle with a given origin and square radius. /// </summary> internal static bool CircleRayCast( VoltShape shape, VoltVector2 shapeOrigin, Fix64 sqrRadius, ref VoltRayCast ray, ref VoltRayResult result) { VoltVector2 toOrigin = shapeOrigin - ray.origin; if (toOrigin.sqrMagnitude < sqrRadius) { result.SetContained(shape); return(true); } Fix64 slope = VoltVector2.Dot(toOrigin, ray.direction); if (slope < Fix64.Zero) { return(false); } Fix64 sqrSlope = slope * slope; Fix64 d = sqrRadius + sqrSlope - VoltVector2.Dot(toOrigin, toOrigin); if (d < Fix64.Zero) { return(false); } Fix64 dist = slope - VoltMath.Sqrt(d); if (dist < Fix64.Zero || 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. VoltVector2 normal = (dist * ray.direction - toOrigin).normalized; result.Set(shape, dist, normal); return(true); }
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 = Fix64.One / this.KScalar(bodyA, bodyB, this.normal); this.tMass = Fix64.One / this.KScalar(bodyA, bodyB, this.normal.Left()); this.bias = Contact.BiasDist(penetration); this.jBias = Fix64.Zero; this.restitution = manifold.Restitution * VoltVector2.Dot( this.normal, this.RelativeVelocity(bodyA, bodyB)); }
private bool CircleCastEdges( ref VoltRayCast bodySpaceRay, Fix64 radius, ref VoltRayResult result) { int foundIndex = -1; bool couldBeContained = true; // Pre-compute and initialize values Fix64 shortestDist = Fix64.MaxValue; VoltVector2 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 VoltVector2 extension = curAxis.Normal * radius; VoltVector2 a = this.bodyVertices[i] + extension; VoltVector2 b = this.bodyVertices[(i + 1) % this.countBody] + extension; // Update the check for containment if (couldBeContained == true) { Fix64 proj = VoltVector2.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 > Fix64.Zero) { // See if the point is within the center Vornoi region of the edge Fix64 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 (VoltVector2.Dot(curAxis.Normal, bodySpaceRay.direction) >= Fix64.Zero) { continue; } // See: // https://rootllama.wordpress.com/2014/06/20/ray-line-segment-intersection-test-in-2d/ VoltVector2 v1 = bodySpaceRay.origin - a; VoltVector2 v2 = b - a; Fix64 denominator = VoltVector2.Dot(v2, v3); Fix64 t1 = VoltMath.Cross(v2, v1) / denominator; Fix64 t2 = VoltVector2.Dot(v1, v3) / denominator; if ((t2 >= Fix64.Zero) && (t2 <= Fix64.One) && (t1 > Fix64.Zero) && (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; Fix64 inner = Fix64.MaxValue; Fix64 outer = Fix64.Zero; 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) Fix64 proj = VoltVector2.Dot(curAxis.Normal, bodySpaceRay.origin) - curAxis.Width; // See if the point is outside of any of the axes if (proj > Fix64.Zero) { couldBeContained = false; } // Projection of the ray direction onto the axis normal (use // negative normal because we want to get the penetration length) Fix64 slope = VoltVector2.Dot(-curAxis.Normal, bodySpaceRay.direction); if (slope == Fix64.Zero) { continue; } Fix64 dist = proj / slope; // The ray is pointing opposite the edge normal (towards the edge) if (slope > Fix64.Zero) { 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); }