/// <summary> /// Workhorse for circle-circle collisions, compares origin distance /// to the sum of the two circles' radii, returns a Manifold. /// </summary> /// private static Manifold TestCircles( VoltWorld world, VoltCircle shapeA, VoltShape shapeB, VoltVector2 overrideBCenter, // For testing vertices in circles Fix64 overrideBRadius) { VoltVector2 r = overrideBCenter - shapeA.worldSpaceOrigin; Fix64 min = shapeA.radius + overrideBRadius; Fix64 distSq = r.sqrMagnitude; if (distSq >= min * min) { return(null); } Fix64 dist = VoltMath.Sqrt(distSq); // 최소값을 지정하여 divide by zero 방지 Fix64 distInv = Fix64.One / VoltMath.Max(dist, min / (Fix64)10); VoltVector2 pos = shapeA.worldSpaceOrigin + (Fix64.One / (Fix64)2 + distInv * (shapeA.radius - min / (Fix64)2)) * r; // Build the collision Manifold Manifold manifold = world.AllocateManifold().Assign(world, shapeA, shapeB); manifold.AddContact(pos, distInv * r, dist - min); return(manifold); }
public static VoltAABB CreateMerged(VoltAABB aabb1, VoltAABB aabb2) { return(new VoltAABB( VoltMath.Max(aabb1.top, aabb2.top), VoltMath.Min(aabb1.bottom, aabb2.bottom), VoltMath.Min(aabb1.left, aabb2.left), VoltMath.Max(aabb1.right, aabb2.right))); }
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); }
private static VoltAABB ComputeBounds( VoltVector2[] vertices, int count) { Fix64 top = vertices[0].y; Fix64 bottom = vertices[0].y; Fix64 left = vertices[0].x; Fix64 right = vertices[0].x; for (int i = 1; i < count; i++) { top = VoltMath.Max(top, vertices[i].y); bottom = VoltMath.Min(bottom, vertices[i].y); left = VoltMath.Min(left, vertices[i].x); right = VoltMath.Max(right, vertices[i].x); } return(new VoltAABB(top, bottom, left, right)); }
/// <summary> /// Builds the AABB by combining all the shape AABBs. /// </summary> private void UpdateAABB() { Fix64 top = Fix64.MinValue; Fix64 right = Fix64.MaxValue; Fix64 bottom = Fix64.MaxValue; Fix64 left = Fix64.MinValue; for (int i = 0; i < this.shapeCount; i++) { VoltAABB aabb = this.shapes[i].AABB; top = VoltMath.Max(top, aabb.Top); right = VoltMath.Max(right, aabb.Right); bottom = VoltMath.Min(bottom, aabb.Bottom); left = VoltMath.Min(left, aabb.Left); } this.AABB = new VoltAABB(top, bottom, left, right); }