private Fix64 KScalar( VoltBody bodyA, VoltBody bodyB, VoltVector2 normal) { Fix64 massSum = bodyA.InvMass + bodyB.InvMass; Fix64 r1cnSqr = VoltMath.Square(VoltMath.Cross(this.toA, normal)); Fix64 r2cnSqr = VoltMath.Square(VoltMath.Cross(this.toB, normal)); return (massSum + bodyA.InvInertia * r1cnSqr + bodyB.InvInertia * r2cnSqr); }
private float KScalar( VoltBody bodyA, VoltBody bodyB, Vector2 normal) { float massSum = bodyA.InvMass + bodyB.InvMass; float r1cnSqr = VoltMath.Square(VoltMath.Cross(this.toA, normal)); float r2cnSqr = VoltMath.Square(VoltMath.Cross(this.toB, normal)); return (massSum + bodyA.InvInertia * r1cnSqr + bodyB.InvInertia * r2cnSqr); }
private static Manifold Circle_Polygon( VoltWorld world, VoltCircle circ, VoltPolygon poly) { // Get the axis on the polygon closest to the circle's origin float penetration; int index = Collision.FindAxisMaxPenetration( circ.worldSpaceOrigin, circ.radius, poly, out penetration); if (index < 0) { return(null); } Vector2 a, b; poly.GetEdge(index, out a, out b); Axis axis = poly.GetWorldAxis(index); // If the circle is past one of the two vertices, check it like // a circle-circle intersection where the vertex has radius 0 float d = VoltMath.Cross(axis.Normal, circ.worldSpaceOrigin); if (d > VoltMath.Cross(axis.Normal, a)) { return(Collision.TestCircles(world, circ, poly, a, 0.0f)); } if (d < VoltMath.Cross(axis.Normal, b)) { return(Collision.TestCircles(world, circ, poly, b, 0.0f)); } // Build the collision Manifold Manifold manifold = world.AllocateManifold().Assign(world, circ, poly); Vector2 pos = circ.worldSpaceOrigin - (circ.radius + penetration / 2) * axis.Normal; manifold.AddContact(pos, -axis.Normal, penetration); return(manifold); }
private float ComputeInertia() { float s1 = 0.0f; float s2 = 0.0f; for (int i = 0; i < this.countBody; i++) { Vector2 v = this.bodyVertices[i]; Vector2 u = this.bodyVertices[(i + 1) % this.countBody]; float a = VoltMath.Cross(u, v); float b = v.sqrMagnitude + u.sqrMagnitude + Vector2.Dot(v, u); s1 += a * b; s2 += a; } return(s1 / (6.0f * s2)); }
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)); }
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)); }
protected override bool ShapeQueryCircle( Vector2 bodySpaceOrigin, float radius) { // Get the axis on the polygon closest to the circle's origin float penetration; int foundIndex = Collision.FindAxisMaxPenetration( bodySpaceOrigin, radius, this, out penetration); if (foundIndex < 0) { return(false); } int numVertices = this.countBody; Vector2 a = this.bodyVertices[foundIndex]; Vector2 b = this.bodyVertices[(foundIndex + 1) % numVertices]; Axis axis = this.bodyAxes[foundIndex]; // If the circle is past one of the two vertices, check it like // a circle-circle intersection where the vertex has radius 0 float d = VoltMath.Cross(axis.Normal, bodySpaceOrigin); if (d > VoltMath.Cross(axis.Normal, a)) { return(Collision.TestPointCircleSimple(a, bodySpaceOrigin, radius)); } if (d < VoltMath.Cross(axis.Normal, b)) { return(Collision.TestPointCircleSimple(b, bodySpaceOrigin, radius)); } return(true); }
private bool CircleCastEdges( ref VoltRayCast bodySpaceRay, float radius, ref VoltRayResult result) { int foundIndex = -1; bool couldBeContained = true; // Pre-compute and initialize values float shortestDist = float.MaxValue; Vector2 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 Vector2 extension = curAxis.Normal * radius; Vector2 a = this.bodyVertices[i] + extension; Vector2 b = this.bodyVertices[(i + 1) % this.countBody] + extension; // Update the check for containment if (couldBeContained == true) { float proj = Vector2.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 float 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 (Vector2.Dot(curAxis.Normal, bodySpaceRay.direction) >= 0.0f) { continue; } // See: // https://rootllama.wordpress.com/2014/06/20/ray-line-segment-intersection-test-in-2d/ Vector2 v1 = bodySpaceRay.origin - a; Vector2 v2 = b - a; float denominator = Vector2.Dot(v2, v3); float t1 = VoltMath.Cross(v2, v1) / denominator; float t2 = Vector2.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); }
internal void ApplyBias(TSVector2 j, TSVector2 r) { this.BiasVelocity += this.InvMass * j; this.BiasRotation -= this.InvInertia * VoltMath.Cross(j, r); }
internal void ApplyImpulse(TSVector2 j, TSVector2 r) { this.LinearVelocity += this.InvMass * j; this.AngularVelocity -= this.InvInertia * VoltMath.Cross(j, r); }
public void AddForce(TSVector2 force, TSVector2 point) { this.Force += force; this.Torque += VoltMath.Cross(this.Position - point, force); }