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; } } }
public void HandleCollision(Manifold m, TFRigidbody a, TFRigidbody b) { TFCircleCollider A = (TFCircleCollider)a.coll; TFCircleCollider B = (TFCircleCollider)b.coll; //Calculate translational vector, which is normal FixVec2 normal = ((FixVec2)b.Position) - ((FixVec2)a.Position); Fix distSpr = normal.GetMagnitudeSquared(); Fix radius = A.radius + B.radius; //Not in contact if (distSpr >= radius * radius) { m.contactCount = 0; return; } Fix distance = FixMath.Sqrt(distSpr); m.contactCount = 1; if (distance == Fix.zero) { m.penetration = A.radius; m.normal = new FixVec2(1, 0); m.contacts[0] = (FixVec2)a.Position; } else { m.penetration = radius - distance; m.normal = normal / distance; m.contacts[0] = m.normal * A.radius + ((FixVec2)a.Position); } }
public void GetMagnitudeSquared(float valL, float valR) { var fix = new FixVec2((Fix)valL, (Fix)valR); var magnitude = fix.GetMagnitudeSquared(); var result = (float)magnitude; Assert.AreEqual((valL * valL) + (valR * valR), result, (Double)Fix.Epsilon); }
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); }
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 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(); }