/// <seealso cref="Silver.Weight.Raw.Collide.Collider.Collide(Silver.Weight.Raw.Contact[], Silver.Weight.Raw.Body, Silver.Weight.Raw.Body)"> /// </seealso> public override int Collide(Contact[] contacts, Body bodyA, Body bodyB) { Polygon polyA = (Polygon) bodyA.Shape; Circle circle = (Circle) bodyB.Shape; // TODO: this can be optimized using matrix multiplications and moving only the circle Vector2f[] vertsA = polyA.GetVertices(bodyA.GetPosition(), bodyA.Rotation); Vector2f centroidA = new Vector2f(polyA.GetCentroid()); centroidA.Add(bodyA.GetPosition()); int[][] collPairs = GetCollisionCandidates(vertsA, centroidA, circle.Radius, bodyB.GetPosition()); int noContacts = 0; for (int i = 0; i < collPairs.Length; i++) { if (noContacts >= contacts.Length) return contacts.Length; Vector2f lineStartA = vertsA[collPairs[i][0]]; Vector2f lineEndA = vertsA[(collPairs[i][0] + 1) % vertsA.Length]; Line line = new Line(lineStartA, lineEndA); float dis2 = line.distanceSquared(bodyB.GetPosition()); float r2 = circle.Radius * circle.Radius; if (dis2 < r2) { Vector2f pt = new Vector2f(); line.getClosestPoint(bodyB.GetPosition(), pt); Vector2f normal = new Vector2f(bodyB.GetPosition()); normal.Sub(pt); float sep = circle.Radius - normal.Length(); normal.Normalise(); contacts[noContacts].Separation = - sep; contacts[noContacts].Position = pt; contacts[noContacts].Normal = normal; contacts[noContacts].Feature = new FeaturePair(); noContacts++; } } return noContacts; }
/// <summary> Get the edges from a list of vertices that can Collide with the given circle. /// This uses a sweepline algorithm which is only efficient if some assumptions /// are indeed true. See CPolygonCPolygonCollider for more information. /// /// </summary> /// <param name="vertsA">The vertices of a polygon that is Collided with a circle /// </param> /// <param name="centroid">The center of the polygon /// </param> /// <param name="radius">The radius of the circle /// </param> /// <param name="circlePos">The position (center) of the circle /// </param> /// <returns> The list of edges that can Collide with the circle /// </returns> protected internal virtual int[][] GetCollisionCandidates(Vector2f[] vertsA, ROVector2f centroid, float radius, ROVector2f circlePos) { Vector2f sweepDir = new Vector2f(centroid); sweepDir.Sub(circlePos); sweepDir.Normalise(); //TODO: this normalization might not be necessary EdgeSweep sweep = new EdgeSweep(sweepDir); //vertsA[0], true, true, dist); sweep.AddVerticesToSweep(true, vertsA); float circProj = circlePos.Dot(sweepDir); sweep.Insert(0, false, - radius + circProj); sweep.Insert(0, false, radius + circProj); return sweep.OverlappingEdges; }
/// <summary> Resolve the collision math around an end point /// /// </summary> /// <param name="pos">The position of the contact /// </param> /// <param name="bodyA">The first body in the collision /// </param> /// <param name="bodyB">The second body in the collision /// </param> /// <param name="leftLine">The line to the left of the vertex of collision /// </param> /// <param name="rightLine">The line to the right of the vertex of collision /// </param> /// <param name="contact">The contact to populate /// </param> /// <param name="norm">The normal determined for the line /// </param> /// <param name="i">The index of teh face we're resolving for feature ID /// </param> private void ResolveEndPointCollision(Vector2f pos, Body bodyA, Body bodyB, Vector2f norm, Line leftLine, Line rightLine, Contact contact, int i) { Vector2f start = new Vector2f(pos); Vector2f end = new Vector2f(start); end.Add(norm); rightLine.move(bodyA.GetPosition()); leftLine.move(bodyA.GetPosition()); Line normLine = new Line(start, end); Vector2f rightPoint = normLine.intersect(rightLine); Vector2f leftPoint = normLine.intersect(leftLine); float dis1 = System.Single.MaxValue; if (rightPoint != null) { dis1 = rightPoint.Distance(start) - norm.Length(); } float dis2 = System.Single.MaxValue; if (leftPoint != null) { dis2 = leftPoint.Distance(start) - norm.Length(); } norm.Normalise(); float dis = System.Math.Min(dis1, dis2); contact.Separation = - dis; contact.Position = pos; contact.Normal = norm; contact.Feature = new FeaturePair(i); }
/// <seealso cref="Silver.Weight.Raw.Collide.Collider.Collide(Silver.Weight.Raw.Contact[], Silver.Weight.Raw.Body, Silver.Weight.Raw.Body)"> /// </seealso> public virtual int Collide(Contact[] contacts, Body bodyA, Body bodyB) { int numContacts = 0; Line line = (Line) bodyA.Shape; Box box = (Box) bodyB.Shape; Vector2f lineVec = new Vector2f(line.DX, line.DY); lineVec.Normalise(); Vector2f axis = new Vector2f(- line.DY, line.DX); axis.Normalise(); Vector2f res = new Vector2f(); line.Start.ProjectOntoUnit(axis, res); float linePos = GetProp(res, axis); Vector2f c = MathUtil.Sub(bodyB.GetPosition(), bodyA.GetPosition()); c.ProjectOntoUnit(axis, res); float centre = GetProp(res, axis); Vector2f[] pts = box.GetPoints(bodyB.GetPosition(), bodyB.Rotation); float[] tangent = new float[4]; float[] proj = new float[4]; int outOfRange = 0; for (int i = 0; i < 4; i++) { pts[i].Sub(bodyA.GetPosition()); pts[i].ProjectOntoUnit(axis, res); tangent[i] = GetProp(res, axis); pts[i].ProjectOntoUnit(lineVec, res); proj[i] = GetProp(res, new Vector2f(line.DX, line.DY)); if ((proj[i] >= 1) || (proj[i] <= 0)) { outOfRange++; } } if (outOfRange == 4) { return 0; } Vector2f normal = new Vector2f(axis); if (centre < linePos) { if (!line.BlocksInnerEdge) { return 0; } normal.Scale(- 1); for (int i = 0; i < 4; i++) { if (tangent[i] > linePos) { if (proj[i] < 0) { Vector2f onAxis = new Vector2f(); Line leftLine = new Line(GetPt(pts, i - 1), pts[i]); Line rightLine = new Line(GetPt(pts, i + 1), pts[i]); leftLine.getClosestPoint(line.Start, res); res.ProjectOntoUnit(axis, onAxis); float left = GetProp(onAxis, axis); rightLine.getClosestPoint(line.Start, res); res.ProjectOntoUnit(axis, onAxis); float right = GetProp(onAxis, axis); if ((left > 0) && (right > 0)) { Vector2f pos = new Vector2f(bodyA.GetPosition()); pos.Add(line.Start); ResolveEndPointCollision(pos, bodyA, bodyB, normal, leftLine, rightLine, contacts[numContacts], i); numContacts++; } } else if (proj[i] > 1) { Vector2f onAxis = new Vector2f(); Line leftLine = new Line(GetPt(pts, i - 1), pts[i]); Line rightLine = new Line(GetPt(pts, i + 1), pts[i]); leftLine.getClosestPoint(line.End, res); res.ProjectOntoUnit(axis, onAxis); float left = GetProp(onAxis, axis); rightLine.getClosestPoint(line.End, res); res.ProjectOntoUnit(axis, onAxis); float right = GetProp(onAxis, axis); if ((left > 0) && (right > 0)) { Vector2f pos = new Vector2f(bodyA.GetPosition()); pos.Add(line.End); ResolveEndPointCollision(pos, bodyA, bodyB, normal, leftLine, rightLine, contacts[numContacts], i); numContacts++; } } else { pts[i].ProjectOntoUnit(lineVec, res); res.Add(bodyA.GetPosition()); contacts[numContacts].Separation = - (tangent[i] - linePos); contacts[numContacts].Position = new Vector2f(res); contacts[numContacts].Normal = normal; contacts[numContacts].Feature = new FeaturePair(i); numContacts++; } } } } else { if (!line.BlocksOuterEdge) { return 0; } for (int i = 0; i < 4; i++) { if (tangent[i] < linePos) { if (proj[i] < 0) { Vector2f onAxis = new Vector2f(); Line leftLine = new Line(GetPt(pts, i - 1), pts[i]); Line rightLine = new Line(GetPt(pts, i + 1), pts[i]); leftLine.getClosestPoint(line.Start, res); res.ProjectOntoUnit(axis, onAxis); float left = GetProp(onAxis, axis); rightLine.getClosestPoint(line.Start, res); res.ProjectOntoUnit(axis, onAxis); float right = GetProp(onAxis, axis); if ((left < 0) && (right < 0)) { Vector2f pos = new Vector2f(bodyA.GetPosition()); pos.Add(line.Start); ResolveEndPointCollision(pos, bodyA, bodyB, normal, leftLine, rightLine, contacts[numContacts], i); numContacts++; } } else if (proj[i] > 1) { Vector2f onAxis = new Vector2f(); Line leftLine = new Line(GetPt(pts, i - 1), pts[i]); Line rightLine = new Line(GetPt(pts, i + 1), pts[i]); leftLine.getClosestPoint(line.End, res); res.ProjectOntoUnit(axis, onAxis); float left = GetProp(onAxis, axis); rightLine.getClosestPoint(line.End, res); res.ProjectOntoUnit(axis, onAxis); float right = GetProp(onAxis, axis); if ((left < 0) && (right < 0)) { Vector2f pos = new Vector2f(bodyA.GetPosition()); pos.Add(line.End); ResolveEndPointCollision(pos, bodyA, bodyB, normal, leftLine, rightLine, contacts[numContacts], i); numContacts++; } } else { pts[i].ProjectOntoUnit(lineVec, res); res.Add(bodyA.GetPosition()); contacts[numContacts].Separation = - (linePos - tangent[i]); contacts[numContacts].Position = new Vector2f(res); contacts[numContacts].Normal = normal; contacts[numContacts].Feature = new FeaturePair(); numContacts++; } } } } if (numContacts > 2) { throw new System.SystemException("LineBoxCollision: > 2 contacts"); } return numContacts; }
/// <summary> Precaculate everything and apply initial impulse before the /// simulation Step takes place /// /// </summary> /// <param name="invDT">The amount of time the simulation is being stepped by /// </param> public virtual void PreStep(float invDT) { // calculate the spring's vector (pointing from body1 to body2) and Length spring = new Vector2f(body2.GetPosition()); spring.Add(r2); spring.Sub(body1.GetPosition()); spring.Sub(r1); springLength = spring.Length(); // the spring vector needs to be normalized for ApplyImpulse as well! spring.Normalise(); // calculate the spring's forces // note that although theoretically invDT could never be 0 // but here it can float springConst; if (springLength < minSpringSize || springLength > maxSpringSize) { // Pre-compute anchors, mass matrix, and bias. Matrix2f rot1 = new Matrix2f(body1.Rotation); Matrix2f rot2 = new Matrix2f(body2.Rotation); r1 = MathUtil.Mul(rot1, localAnchor1); r2 = MathUtil.Mul(rot2, localAnchor2); // the mass normal or 'k' float rn1 = r1.Dot(spring); float rn2 = r2.Dot(spring); float kNormal = body1.InvMass + body2.InvMass; kNormal += body1.InvI * (r1.Dot(r1) - rn1 * rn1) + body2.InvI * (r2.Dot(r2) - rn2 * rn2); massNormal = 1 / kNormal; // The spring is broken so apply force to correct it // note that we use biased velocities for this float springImpulse = invDT != 0?brokenSpringConst * (springLength - springSize) / invDT:0; Vector2f impulse = MathUtil.Scale(spring, springImpulse); body1.AdjustBiasedVelocity(MathUtil.Scale(impulse, body1.InvMass)); body1.AdjustBiasedAngularVelocity((body1.InvI * MathUtil.Cross(r1, impulse))); body2.AdjustBiasedVelocity(MathUtil.Scale(impulse, - body2.InvMass)); body2.AdjustBiasedAngularVelocity(- (body2.InvI * MathUtil.Cross(r2, impulse))); isBroken = true; return ; } else if (springLength < springSize) { springConst = compressedSpringConst; isBroken = false; } else { // if ( springLength >= springSize ) springConst = stretchedSpringConst; isBroken = false; } float springImpulse2 = invDT != 0?springConst * (springLength - springSize) / invDT:0; // apply the spring's forces Vector2f impulse2 = MathUtil.Scale(spring, springImpulse2); body1.AdjustVelocity(MathUtil.Scale(impulse2, body1.InvMass)); body1.AdjustAngularVelocity((body1.InvI * MathUtil.Cross(r1, impulse2))); body2.AdjustVelocity(MathUtil.Scale(impulse2, - body2.InvMass)); body2.AdjustAngularVelocity(- (body2.InvI * MathUtil.Cross(r2, impulse2))); }