/// <summary> /// is rotating from A to B Counter-clockwise? /// </summary> /// <param name="A">vector A</param> /// <param name="B">vector B</param> /// <returns>true = CCW or opposite (180 degrees), false = CW</returns> public static bool IsCCW(Vector2 a, Vector2 b) { Vector2 perp = VectorHelper.Perpendicular(a); float dot; Vector2.Dot(ref b, ref perp, out dot); return(dot >= 0.0f); }
/// <summary> /// is rotating from A to B Counter-Clockwise? /// </summary> /// <param name="A">vector A</param> /// <param name="B">vector B</param> /// <returns>true = CCW or opposite (180 degrees), false = CW</returns> public static bool IsCCW(ref Vector2 A, ref Vector2 B) { Vector2 perp = new Vector2(); VectorHelper.Perpendicular(ref A, ref perp); float dot; Vector2.Dot(ref B, ref perp, out dot); return(dot >= 0.0f); }
public override void ApplyInternalForces(double elapsed) { base.ApplyInternalForces(elapsed); // internal forces based on pressure equations. we need 2 loops to do this. one to find the overall volume of the // body, and 1 to apply forces. we will need the normals for the edges in both loops, so we will cache them and remember them. volume = 0f; for (int i = 0; i < count; i++) { int prev = (i > 0) ? i - 1 : count - 1; int next = (i < count - 1) ? i + 1 : 0; // currently we are talking about the edge from i --> j. // first calculate the volume of the body, and cache normals as we go. Vector2 edge1N = new Vector2(); edge1N.X = pointmass_list[i].position.X - pointmass_list[prev].position.X; edge1N.Y = pointmass_list[i].position.Y - pointmass_list[prev].position.Y; VectorHelper.Perpendicular(ref edge1N); Vector2 edge2N = new Vector2(); edge2N.X = pointmass_list[next].position.X - pointmass_list[i].position.X; edge2N.Y = pointmass_list[next].position.Y - pointmass_list[i].position.Y; VectorHelper.Perpendicular(ref edge2N); Vector2 norm = new Vector2(); norm.X = edge1N.X + edge2N.X; norm.Y = edge1N.Y + edge2N.Y; float nL = (float)Math.Sqrt((norm.X * norm.X) + (norm.Y * norm.Y)); if (nL > 0.001f) { norm.X /= nL; norm.Y /= nL; } float edgeL = (float)Math.Sqrt((edge2N.X * edge2N.X) + (edge2N.Y * edge2N.Y)); // cache normal and edge length normal_list[i] = norm; edgelength_list[i] = edgeL; float xdist = Math.Abs(pointmass_list[i].position.X - pointmass_list[next].position.X); float volumeProduct = xdist * Math.Abs(norm.X) * edgeL; // add to volume volume += 0.5f * volumeProduct; } // now loop through, adding forces! float invVolume = 1f / volume; for (int i = 0; i < count; i++) { int j = (i < count - 1) ? i + 1 : 0; float pressureV = (invVolume * edgelength_list[i] * pressure); pointmass_list[i].force.X += normal_list[i].X * pressureV; pointmass_list[i].force.Y += normal_list[i].Y * pressureV; pointmass_list[j].force.X += normal_list[j].X * pressureV; pointmass_list[j].force.Y += normal_list[j].Y * pressureV; } }
public static List <CollisionInfo> Intersects(Body body_a, Body body_b) { List <CollisionInfo> data = new List <CollisionInfo>(); int bApmCount = body_a.count; int bBpmCount = body_b.count; BoundingSquare boxB = body_b.aabb; // check all PointMasses on bodyA for collision against bodyB. if there is a collision, return detailed info. CollisionInfo infoAway = new CollisionInfo(); CollisionInfo infoSame = new CollisionInfo(); for (int i = 0; i < bApmCount; i++) { Vector2 pt = body_a.pointmass_list[i].position; // early out - if this point is outside the bounding box for bodyB, skip it! if (!boxB.Contains(pt.X, pt.Y)) { continue; } // early out - if this point is not inside bodyB, skip it! if (!body_b.Contains(ref pt)) { continue; } int prevPt = (i > 0) ? i - 1 : bApmCount - 1; int nextPt = (i < bApmCount - 1) ? i + 1 : 0; Vector2 prev = body_a.pointmass_list[prevPt].position; Vector2 next = body_a.pointmass_list[nextPt].position; // now get the normal for this point. (NOT A UNIT VECTOR) Vector2 fromPrev = new Vector2(); fromPrev.X = pt.X - prev.X; fromPrev.Y = pt.Y - prev.Y; Vector2 toNext = new Vector2(); toNext.X = next.X - pt.X; toNext.Y = next.Y - pt.Y; Vector2 ptNorm = new Vector2(); ptNorm.X = fromPrev.X + toNext.X; ptNorm.Y = fromPrev.Y + toNext.Y; VectorHelper.Perpendicular(ref ptNorm); // this point is inside the other body. now check if the edges on either side intersect with and edges on bodyB. float closestAway = 100000.0f; float closestSame = 100000.0f; infoAway.Clear(); infoAway.body_a = body_a; infoAway.pointmass_a = body_a.pointmass_list[i]; infoAway.body_b = body_b; infoSame.Clear(); infoSame.body_a = body_a; infoSame.pointmass_a = body_a.pointmass_list[i]; infoSame.body_b = body_b; bool found = false; int b1 = 0; int b2 = 1; for (int j = 0; j < bBpmCount; j++) { Vector2 hitPt; Vector2 norm; float edgeD; b1 = j; if (j < bBpmCount - 1) { b2 = j + 1; } else { b2 = 0; } Vector2 pt1 = body_b.pointmass_list[b1].position; Vector2 pt2 = body_b.pointmass_list[b2].position; // quick test of distance to each point on the edge, if both are greater than current mins, we can skip! float distToA = ((pt1.X - pt.X) * (pt1.X - pt.X)) + ((pt1.Y - pt.Y) * (pt1.Y - pt.Y)); float distToB = ((pt2.X - pt.X) * (pt2.X - pt.X)) + ((pt2.Y - pt.Y) * (pt2.Y - pt.Y)); if ((distToA > closestAway) && (distToA > closestSame) && (distToB > closestAway) && (distToB > closestSame)) { continue; } // test against this edge. float dist = body_b.GetClosestPointOnEdgeSquared(pt, j, out hitPt, out norm, out edgeD); // only perform the check if the normal for this edge is facing AWAY from the point normal. float dot; Vector2.Dot(ref ptNorm, ref norm, out dot); if (dot <= 0f) { if (dist < closestAway) { closestAway = dist; infoAway.pointmass_b = body_b.pointmass_list[b1]; infoAway.pointmass_c = body_b.pointmass_list[b2]; infoAway.edge_distance = edgeD; infoAway.point = hitPt; infoAway.normal = norm; infoAway.penetration = dist; found = true; } } else { if (dist < closestSame) { closestSame = dist; infoSame.pointmass_b = body_b.pointmass_list[b1]; infoSame.pointmass_c = body_b.pointmass_list[b2]; infoSame.edge_distance = edgeD; infoSame.point = hitPt; infoSame.normal = norm; infoSame.penetration = dist; } } } // we've checked all edges on BodyB. if ((found) && (closestAway > 0.3f) && (closestSame < closestAway)) { infoSame.penetration = (float)Math.Sqrt(infoSame.penetration); data.Add(infoSame); } else { infoAway.penetration = (float)Math.Sqrt(infoAway.penetration); data.Add(infoAway); } } return(data); }
public float GetClosestPointOnEdgeSquared(Vector2 point, int edgeNum, out Vector2 hitPt, out Vector2 normal, out float edgeD) { hitPt = new Vector2(); hitPt.X = 0f; hitPt.Y = 0f; normal = new Vector2(); normal.X = 0f; normal.Y = 0f; edgeD = 0f; float dist = 0f; Vector2 ptA = pointmass_list[edgeNum].position; Vector2 ptB = new Vector2(); if (edgeNum < (count - 1)) { ptB = pointmass_list[edgeNum + 1].position; } else { ptB = pointmass_list[0].position; } Vector2 toP = new Vector2(); toP.X = point.X - ptA.X; toP.Y = point.Y - ptA.Y; Vector2 E = new Vector2(); E.X = ptB.X - ptA.X; E.Y = ptB.Y - ptA.Y; // get the length of the edge, and use that to normalize the vector. float edgeLength = (float)Math.Sqrt((E.X * E.X) + (E.Y * E.Y)); if (edgeLength > 0.00001f) { E.X /= edgeLength; E.Y /= edgeLength; } // normal Vector2 n = new Vector2(); VectorHelper.Perpendicular(ref E, ref n); // calculate the distance! float x; Vector2.Dot(ref toP, ref E, out x); if (x <= 0.0f) { // x is outside the line segment, distance is from pt to ptA. //dist = (pt - ptA).Length(); Vector2.DistanceSquared(ref point, ref ptA, out dist); hitPt = ptA; edgeD = 0f; normal = n; } else if (x >= edgeLength) { // x is outside of the line segment, distance is from pt to ptB. //dist = (pt - ptB).Length(); Vector2.DistanceSquared(ref point, ref ptB, out dist); hitPt = ptB; edgeD = 1f; normal = n; } else { // point lies somewhere on the line segment. Vector3 toP3 = new Vector3(); toP3.X = toP.X; toP3.Y = toP.Y; Vector3 E3 = new Vector3(); E3.X = E.X; E3.Y = E.Y; //dist = Math.Abs(Vector3.Cross(toP3, E3).Z); Vector3.Cross(ref toP3, ref E3, out E3); dist = Math.Abs(E3.Z * E3.Z); hitPt.X = ptA.X + (E.X * x); hitPt.Y = ptA.Y + (E.Y * x); edgeD = x / edgeLength; normal = n; } return(dist); }
public void Update(double elapsed) { if (!initialized) { Initialize(); } penetration_count = 0; collision_list.Clear(); for (int i = 0; i < body_list.Count; i++) { body_list[i].Update(elapsed); UpdateBitmask(body_list[i]); } // update chains for (int i = 0; i < chain_list.Count; i++) { chain_list[i].Update(elapsed); } // now check for collision. // inter-body collision! for (int i = 0; i < body_list.Count; i++) { for (int j = i + 1; j < body_list.Count; j++) { if (body_list[i].is_static && body_list[j].is_static) { continue; } // grid-based early out. if (((body_list[i].bitmaskx.mask & body_list[j].bitmaskx.mask) == 0) && ((body_list[i].bitmasky.mask & body_list[j].bitmasky.mask) == 0)) { continue; } // broad-phase collision via AABB. if (!(body_list[i].aabb).Intersects(ref (body_list[j].aabb))) { continue; } if (on_aabb_collision != null) { this.on_aabb_collision(body_list[i], body_list[j]); } // okay, the AABB's of these 2 are intersecting. now check for collision of A against B. collision_list.AddRange(Collision.Intersects(body_list[j], body_list[i])); collision_list.AddRange(Collision.Intersects(body_list[i], body_list[j])); } } // now handle all collisions found during the update at once. // handle all collisions! for (int i = 0; i < collision_list.Count; i++) { CollisionInfo info = collision_list[i]; PointMass A = info.pointmass_a; PointMass B1 = info.pointmass_b; PointMass B2 = info.pointmass_c; if (on_collision != null) { this.on_collision(info.body_a, info.body_b, info); } // velocity changes as a result of collision. Vector2 bVel = new Vector2(); bVel.X = (B1.velocity.X + B2.velocity.X) * 0.5f; bVel.Y = (B1.velocity.Y + B2.velocity.Y) * 0.5f; Vector2 relVel = new Vector2(); relVel.X = A.velocity.X - bVel.X; relVel.Y = A.velocity.Y - bVel.Y; float relDot; Vector2.Dot(ref relVel, ref info.normal, out relDot); if (on_penetration != null) { if (info.penetration > penetration_threshold) { this.on_penetration(info.body_a, info.body_b); } } if (info.penetration > 0.3f) { penetration_count++; continue; } float b1inf = 1.0f - info.edge_distance; float b2inf = info.edge_distance; float b2MassSum = ((float.IsPositiveInfinity(B1.mass)) || (float.IsPositiveInfinity(B2.mass))) ? float.PositiveInfinity : (B1.mass + B2.mass); float massSum = A.mass + b2MassSum; float Amove; float Bmove; if (float.IsPositiveInfinity(A.mass)) { Amove = 0f; Bmove = (info.penetration) + 0.001f; } else if (float.IsPositiveInfinity(b2MassSum)) { Amove = (info.penetration) + 0.001f; Bmove = 0f; } else { Amove = (info.penetration * (b2MassSum / massSum)); Bmove = (info.penetration * (A.mass / massSum)); } float B1move = Bmove * b1inf; float B2move = Bmove * b2inf; float AinvMass = (float.IsPositiveInfinity(A.mass)) ? 0f : 1f / A.mass; float BinvMass = (float.IsPositiveInfinity(b2MassSum)) ? 0f : 1f / b2MassSum; float jDenom = AinvMass + BinvMass; Vector2 numV = new Vector2(); float elas = elasticity; numV.X = relVel.X * elas; numV.Y = relVel.Y * elas; float jNumerator; Vector2.Dot(ref numV, ref info.normal, out jNumerator); jNumerator = -jNumerator; float j = jNumerator / jDenom; if (!float.IsPositiveInfinity(A.mass)) { A.position.X += info.normal.X * Amove; A.position.Y += info.normal.Y * Amove; } if (!float.IsPositiveInfinity(B1.mass)) { B1.position.X -= info.normal.X * B1move; B1.position.Y -= info.normal.Y * B1move; } if (!float.IsPositiveInfinity(B2.mass)) { B2.position.X -= info.normal.X * B2move; B2.position.Y -= info.normal.Y * B2move; } Vector2 tangent = new Vector2(); VectorHelper.Perpendicular(ref info.normal, ref tangent); float fNumerator; Vector2.Dot(ref relVel, ref tangent, out fNumerator); fNumerator *= friction; float f = fNumerator / jDenom; // adjust velocity if relative velocity is moving toward each other. if (relDot <= 0.0001f) { if (!float.IsPositiveInfinity(A.mass)) { A.velocity.X += (info.normal.X * (j / A.mass)) - (tangent.X * (f / A.mass)); A.velocity.Y += (info.normal.Y * (j / A.mass)) - (tangent.Y * (f / A.mass)); } if (!float.IsPositiveInfinity(b2MassSum)) { B1.velocity.X -= (info.normal.X * (j / b2MassSum) * b1inf) - (tangent.X * (f / b2MassSum) * b1inf); B1.velocity.Y -= (info.normal.Y * (j / b2MassSum) * b1inf) - (tangent.Y * (f / b2MassSum) * b1inf); } if (!float.IsPositiveInfinity(b2MassSum)) { B2.velocity.X -= (info.normal.X * (j / b2MassSum) * b2inf) - (tangent.X * (f / b2MassSum) * b2inf); B2.velocity.Y -= (info.normal.Y * (j / b2MassSum) * b2inf) - (tangent.Y * (f / b2MassSum) * b2inf); } //info.body_a.UpdateBodyPositionVelocityForce(elapsed); // info.body_b.UpdateBodyPositionVelocityForce(elapsed); } } for (int i = 0; i < body_list.Count; i++) { body_list[i].UpdateBodyPositionVelocityForce(elapsed); } }