/// <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> Configure the line
///
/// </summary>
/// <param name="start">The start point of the line
/// </param>
/// <param name="end">The end point of the line
/// </param>
public virtual void Reconfigure(ROVector2f start, ROVector2f end)
{
this.start = start;
this.end = end;
vec = new Vector2f(end);
vec.Sub(start);
lenSquared = vec.Length();
lenSquared *= lenSquared;
}
/// <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)));
}
/// <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);
}
/// <summary> Create a joint holding two bodies together
///
/// </summary>
/// <param name="b1">The first body attached to the joint
/// </param>
/// <param name="b2">The second body attached to the joint
/// </param>
/// <param name="anchor1">The location of the attachment to the first body, in absolute coordinates.
/// </param>
/// <param name="anchor2">The location of the attachment to the second body, in absolute coordinates.
/// </param>
public SpringJoint(Body b1, Body b2, ROVector2f anchor1, ROVector2f anchor2)
{
id = NEXT_ID++;
stretchedSpringConst = 100;
compressedSpringConst = 100;
brokenSpringConst = 100;
Vector2f spring = new Vector2f(anchor1);
spring.Sub(anchor2);
springSize = spring.Length();
minSpringSize = 0;
maxSpringSize = 2 * springSize;
Reconfigure(b1, b2, anchor1, anchor2);
}