public void initialize() { // Calculate average bounciness/restitution e = FixMath.Min(A.Bounciness, B.Bounciness); // Calculate static & dynamic friction sf = FixMath.Sqrt(A.StaticFriction * A.StaticFriction + B.StaticFriction * B.StaticFriction); df = FixMath.Sqrt(A.DynamicFriction * A.DynamicFriction + B.DynamicFriction * B.DynamicFriction); for (int i = 0; i < contactCount; i++) { //Calculate radii from COM to contact FixVec2 ra = contacts[i] -= A.info.position; FixVec2 rb = contacts[i] -= B.info.position; //? FixVec2 rv = B.info.velocity + FixVec2.Cross(B.info.angularVelocity, rb) - A.info.velocity - FixVec2.Cross(A.info.angularVelocity, ra); // Determine if we should perform a resting collision or not // The idea is if the only thing moving this object is gravity, // then the collision should be performed without any restitution if (rv.GetMagnitudeSquared() < TFPhysics.instance.resting) { e = 0; } } }
internal TFRaycastHit2D Raycast(ITreeRaycastCallback callback, FixVec2 pointA, FixVec2 pointB, TFLayerMask mask) { TFRaycastHit2D hit = new TFRaycastHit2D(); FixVec2 r = pointB - pointA; if (r.GetMagnitudeSquared() <= Fix.zero) { return(hit); } r.Normalize(); // v is perpendicular to the segment. FixVec2 v = FixVec2.Cross(Fix.one, r); FixVec2 abs_v = FixVec2.Abs(v); // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) Fix maxFraction = Fix.one; // Build a bounding box for the segment. AABB segmentAABB = new AABB(); FixVec2 t = pointA + maxFraction * (pointB - pointA); segmentAABB.min = FixVec2.Min(pointA, t); segmentAABB.max = FixVec2.Max(pointA, t); Stack <int> stack = new Stack <int>(); stack.Push(rootIndex); List <TFRaycastOutput> hitNodes = new List <TFRaycastOutput>(2); while (stack.Count > 0) { var nodeId = stack.Pop(); if (nodeId == nullNode) { continue; } var node = nodes[nodeId]; if (!node.aabb.Overlaps(segmentAABB)) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) var c = node.aabb.GetCenter(); var h = node.aabb.GetExtents(); var separation = FixMath.Abs(FixVec2.Dot(v, pointA - c)) - FixVec2.Dot(abs_v, h); if (separation > Fix.zero) { continue; } if (node.IsLeaf()) { // If value is >= 0, then we hit the node. TFRaycastHit2D rHit; Fix value = callback.RayCastCallback(pointA, pointB, maxFraction, nodeId, out rHit, mask); if (value == Fix.zero) { // The client has terminated the ray cast. if (rHit) { // We actually hit the node, add it to the list. hitNodes.Add(new TFRaycastOutput(nodeId, rHit)); } break; } if (value == maxFraction) { if (rHit) { // We actually hit the node, add it to the list. hitNodes.Add(new TFRaycastOutput(nodeId, rHit)); } } else if (value > Fix.zero) { if (rHit) { // We actually hit the node, add it to the list. hitNodes.Add(new TFRaycastOutput(nodeId, rHit)); } // Update segment bounding box. maxFraction = value; FixVec2 g = pointA + maxFraction * (pointB - pointA); segmentAABB.min = FixVec2.Min(pointA, g); segmentAABB.max = FixVec2.Max(pointA, g); } } else { stack.Push(node.leftChildIndex); stack.Push(node.rightChildIndex); } } // Decide which node was the closest to the starting point. Fix closestNode = maxFraction; for (int i = 0; i < hitNodes.Count; i++) { if (hitNodes[i].hit.fraction < closestNode) { closestNode = hitNodes[i].hit.fraction; hit = hitNodes[i].hit; } } return(hit); }
public void ApplyImpulse(FixVec2 impulse, FixVec2 contactVector) { info.velocity += impulse * invMass; info.angularVelocity += invInertia * FixVec2.Cross(contactVector, impulse); }
internal void Raycast(ITreeRaycastCallback callback, FixVec2 pointA, FixVec2 pointB) { FixVec2 r = pointB - pointA; if (r.GetMagnitudeSquared() <= Fix.zero) { return; } r.Normalize(); // v is perpendicular to the segment. FixVec2 v = FixVec2.Cross(Fix.one, r); FixVec2 abs_v = FixVec2.Abs(v); // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) Fix maxFraction = Fix.one; // Build a bounding box for the segment. AABB segmentAABB = new AABB(); FixVec2 t = pointA + maxFraction * (pointB - pointA); segmentAABB.min = FixVec2.Min(pointA, t); segmentAABB.max = FixVec2.Max(pointA, t); Stack <int> stack = new Stack <int>(); stack.Push(rootIndex); while (stack.Count > 0) { var nodeId = stack.Pop(); if (nodeId == nullNode) { continue; } var node = nodes[nodeId]; if (!node.aabb.Overlaps(segmentAABB)) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) var c = node.aabb.GetCenter(); var h = node.aabb.GetExtents(); var separation = FixMath.Abs(FixVec2.Dot(v, pointA - c)) - FixVec2.Dot(abs_v, h); if (separation > Fix.zero) { continue; } if (node.IsLeaf()) { Fix value = callback.RayCastCallback(pointA, pointB, maxFraction, nodeId); if (value == Fix.zero) { // The client has terminated the ray cast. return; } if (value > Fix.zero) { // Update segment bounding box. maxFraction = value; FixVec2 g = pointA + maxFraction * (pointB - pointA); segmentAABB.min = FixVec2.Min(pointA, g); segmentAABB.max = FixVec2.Max(pointA, g); } } else { stack.Push(node.leftChildIndex); stack.Push(node.rightChildIndex); } } }
public void ApplyImpulse() { // Early out and positional correct if both objects have infinite mass if (A.invMass + B.invMass == 0) { InfiniteMassCorrection(); return; } for (int i = 0; i < contactCount; ++i) { // Calculate radii from COM to contact FixVec2 ra = contacts[i] - A.Position; FixVec2 rb = contacts[i] - B.Position; //Relative velocity FixVec2 rv = B.info.velocity + FixVec2.Cross(B.info.angularVelocity, rb) - A.info.velocity - FixVec2.Cross(A.info.angularVelocity, ra); //Relative velocity along the normal Fix contactVel = rv.Dot(normal); if (contactVel > 0) { return; } Fix raCrossN = FixVec2.Cross(ra, normal); Fix rbCrossN = FixVec2.Cross(rb, normal); Fix invMassSum = A.invMass + B.invMass + (raCrossN * raCrossN) * A.invInertia + (rbCrossN * rbCrossN) * B.invInertia; // Calculate impulse scalar Fix j = -(Fix.one + e) * contactVel; j /= invMassSum; j /= contactCount; // Apply impulse FixVec2 impulse = normal * j; A.ApplyImpulse(-impulse, ra); B.ApplyImpulse(impulse, rb); // Friction Impulse rv = B.info.velocity + FixVec2.Cross(B.info.angularVelocity, rb) - A.info.velocity - FixVec2.Cross(A.info.angularVelocity, ra); FixVec2 t = rv - (normal * FixVec2.Dot(rv, normal)); t = t.Normalized(); // j tangent magnitude Fix jt = -FixVec2.Dot(rv, t); jt /= invMassSum; jt /= contactCount; //Don't apply tiny friction impulses if (FixMath.Abs(jt) <= Fix.zero) { return; } // Coulumb's law FixVec2 tangentImpulse; if (FixMath.Abs(jt) < j * sf) { tangentImpulse = t * jt; } else { tangentImpulse = t * -j * df; } // Apply friction impulse A.ApplyImpulse(-tangentImpulse, ra); B.ApplyImpulse(tangentImpulse, rb); } }
public void SetVertices(List <FixVec2> verts) { // Find the right most point on the hull int rightMost = 0; Fix highestXCoord = verts[0].x; for (int i = 1; i < verts.Count; ++i) { Fix x = verts[i].x; if (x > highestXCoord) { highestXCoord = x; rightMost = i; } // If matching x then take farthest negative y else if (x == highestXCoord) { if (verts[i].y < verts[rightMost].y) { rightMost = i; } } } int[] hull = new int[MAX_POLY_VERTEX_COUNT]; int outCount = 0; int indexHull = rightMost; for (; ;) { hull[outCount] = indexHull; // Search for next index that wraps around the hull // by computing cross products to find the most counter-clockwise // vertex in the set, given the previos hull index int nextHullIndex = 0; for (int i = 1; i < verts.Count; ++i) { // Skip if same coordinate as we need three unique // points in the set to perform a cross product if (nextHullIndex == indexHull) { nextHullIndex = i; continue; } // Cross every set of three unique vertices // Record each counter clockwise third vertex and add // to the output hull // See : http://www.oocities.org/pcgpe/math2d.html FixVec2 e1 = verts[nextHullIndex] - verts[hull[outCount]]; FixVec2 e2 = verts[i] - verts[hull[outCount]]; Fix c = FixVec2.Cross(e1, e2); if (c < Fix.zero) { nextHullIndex = i; } // Cross product is zero then e vectors are on same line // therefore want to record vertex farthest along that line if (c == Fix.zero && e2.GetMagnitudeSquared() > e1.GetMagnitudeSquared()) { nextHullIndex = i; } } ++outCount; indexHull = nextHullIndex; // Conclude algorithm upon wrap-around if (nextHullIndex == rightMost) { break; } } // Copy vertices into shape's vertices for (int i = 0; i < vertices.Count; ++i) { vertices[i] = verts[hull[i]]; } CalculateNormals(); }