/// <summary> Reconfigure this joint
///
/// </summary>
/// <param name="b1">The first body attached to this joint
/// </param>
/// <param name="b2">The second body attached to this joint
/// </param>
/// <param name="anchor">The static anchor point between the joints
/// </param>
public virtual void Reconfigure(Body b1, Body b2, Vector2f anchor)
{
body1 = b1;
body2 = b2;
Matrix2f rot1 = new Matrix2f(body1.Rotation);
Matrix2f rot2 = new Matrix2f(body2.Rotation);
Matrix2f rot1T = rot1.Transpose();
Matrix2f rot2T = rot2.Transpose();
Vector2f a1 = new Vector2f(anchor);
a1.Sub(body1.GetPosition());
localAnchor1 = MathUtil.Mul(rot1T, a1);
Vector2f a2 = new Vector2f(anchor);
a2.Sub(body2.GetPosition());
localAnchor2 = MathUtil.Mul(rot2T, a2);
accumulatedImpulse.Reconfigure(0.0f, 0.0f);
relaxation = 1.0f;
}
/// <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)
{
// 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);
// deltaV = deltaV0 + K * impulse
// invM = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
Matrix2f K1 = new Matrix2f();
K1.col1.x = body1.InvMass + body2.InvMass; K1.col2.x = 0.0f;
K1.col1.y = 0.0f; K1.col2.y = body1.InvMass + body2.InvMass;
Matrix2f K2 = new Matrix2f();
K2.col1.x = body1.InvI * r1.y * r1.y; K2.col2.x = (- body1.InvI) * r1.x * r1.y;
K2.col1.y = (- body1.InvI) * r1.x * r1.y; K2.col2.y = body1.InvI * r1.x * r1.x;
Matrix2f K3 = new Matrix2f();
K3.col1.x = body2.InvI * r2.y * r2.y; K3.col2.x = (- body2.InvI) * r2.x * r2.y;
K3.col1.y = (- body2.InvI) * r2.x * r2.y; K3.col2.y = body2.InvI * r2.x * r2.x;
Matrix2f K = MathUtil.Add(MathUtil.Add(K1, K2), K3);
M = K.Invert();
Vector2f p1 = new Vector2f(body1.GetPosition());
p1.Add(r1);
Vector2f p2 = new Vector2f(body2.GetPosition());
p2.Add(r2);
Vector2f dp = new Vector2f(p2);
dp.Sub(p1);
bias = new Vector2f(dp);
bias.Scale(- 0.1f);
bias.Scale(invDT);
// Apply accumulated impulse.
accumulatedImpulse.Scale(relaxation);
if (!body1.Static)
{
Vector2f accum1 = new Vector2f(accumulatedImpulse);
accum1.Scale(- body1.InvMass);
body1.AdjustVelocity(accum1);
body1.AdjustAngularVelocity(- (body1.InvI * MathUtil.Cross(r1, accumulatedImpulse)));
}
if (!body2.Static)
{
Vector2f accum2 = new Vector2f(accumulatedImpulse);
accum2.Scale(body2.InvMass);
body2.AdjustVelocity(accum2);
body2.AdjustAngularVelocity(body2.InvI * MathUtil.Cross(r2, accumulatedImpulse));
}
}
/// <summary> Reconfigure this joint
///
/// </summary>
/// <param name="b1">The first body attached to this joint
/// </param>
/// <param name="b2">The second body attached to this 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 virtual void Reconfigure(Body b1, Body b2, ROVector2f anchor1, ROVector2f anchor2)
{
body1 = b1;
body2 = b2;
Matrix2f rot1 = new Matrix2f(body1.Rotation);
Matrix2f rot1T = rot1.Transpose();
Vector2f a1 = new Vector2f(anchor1);
a1.Sub(body1.GetPosition());
localAnchor1 = MathUtil.Mul(rot1T, a1);
Matrix2f rot2 = new Matrix2f(body2.Rotation);
Matrix2f rot2T = rot2.Transpose();
Vector2f a2 = new Vector2f(anchor2);
a2.Sub(body2.GetPosition());
localAnchor2 = MathUtil.Mul(rot2T, a2);
}
/// <summary> Get the current positon of a set of points
///
/// </summary>
/// <param name="pos">The centre of the box
/// </param>
/// <param name="rotation">The rotation of the box
/// </param>
/// <returns> The points building up a box at this position and rotation
/// </returns>
public virtual Vector2f[] GetPoints(ROVector2f pos, float rotation)
{
Matrix2f R = new Matrix2f(rotation);
Vector2f[] pts = new Vector2f[4];
ROVector2f h = MathUtil.Scale(Size, 0.5f);
pts[0] = MathUtil.Mul(R, new Vector2f(- h.X, - h.Y));
pts[0].Add(pos);
pts[1] = MathUtil.Mul(R, new Vector2f(h.X, - h.Y));
pts[1].Add(pos);
pts[2] = MathUtil.Mul(R, new Vector2f(h.X, h.Y));
pts[2].Add(pos);
pts[3] = MathUtil.Mul(R, new Vector2f(- h.X, h.Y));
pts[3].Add(pos);
return pts;
}
/// <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)));
}
