void Joint::PreStep(float inv_dt)
{
// Pre-compute anchors, mass matrix, and bias.
Mat22 Rot1(body1->rotation);
Mat22 Rot2(body2->rotation);
r1 = Rot1 * localAnchor1;
r2 = 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]
Mat22 K1;
K1.col1.x = body1->invMass + body2->invMass; K1.col2.x = 0.0f;
K1.col1.y = 0.0f; K1.col2.y = body1->invMass + body2->invMass;
Mat22 K2;
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;
Mat22 K3;
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;
Mat22 K = K1 + K2 + K3;
K.col1.x += softness;
K.col2.y += softness;
M = K.Invert();
Vec2 p1 = body1->position + r1;
Vec2 p2 = body2->position + r2;
Vec2 dp = p2 - p1;
if (World::positionCorrection)
{
bias = -biasFactor * inv_dt * dp;
}
else
{
bias.Set(0.0f, 0.0f);
}
if (World::warmStarting)
{
// Apply accumulated impulse.
body1->velocity -= body1->invMass * P;
body1->angularVelocity -= body1->invI * Cross(r1, P);
body2->velocity += body2->invMass * P;
body2->angularVelocity += body2->invI * Cross(r2, P);
}
else
{
P.Set(0.0f, 0.0f);
}
}
bool SATCollide(SimBody *body1, SimBody *body2, float2 &N, f32 &t)
{
SimBody &a = *body1;
SimBody &b = *body2;
if(a.vertices.size() < 2 && b.vertices.size() < 2) return false;
Mat22 OA = Mat22::RotationMatrix(a.rotation_in_rads);
Mat22 OB = Mat22::RotationMatrix(b.rotation_in_rads);
Mat22 OB_T = OB.Transpose();
Mat22 xOrient = OA * OB_T;
float2 xOffset = (a.position - b.position) * OB_T;
const u32 MAX_SEPERATING_AXIS = 16;
float2 xAxis[MAX_SEPERATING_AXIS];
f32 taxis[MAX_SEPERATING_AXIS];
u32 axisCount = 0;
for(u32 i=0;i<a.seperatingAxis.size();++i)
{
xAxis[axisCount] = a.seperatingAxis[i] * xOrient;
if(!IntervalIntersect(a.vertices, b.vertices, xAxis[axisCount],
xOffset, xOrient, taxis[axisCount], t))
{
return false;
}
++axisCount;
};
for(u32 i=0;i<b.seperatingAxis.size();++i)
{
xAxis[axisCount] = b.seperatingAxis[i];
if(!IntervalIntersect(a.vertices, b.vertices, xAxis[axisCount],
xOffset, xOrient, taxis[axisCount], t))
{
return false;
}
++axisCount;
};
if(!GetMinimumTranslationVector(xAxis, taxis, axisCount, N, t))
{
return false;
}
f32 D = N.dot(xOffset);
N = D < 0.0f ? -N : N;
N = N * OB;
return true;
};
static void ComputeIncidentEdge(ClipVertex c[2], const Vec2& h, const Vec2& pos,
const Mat22& Rot, const Vec2& normal)
{
// The normal is from the reference box. Convert it
// to the incident boxe's frame and flip sign.
Mat22 RotT = Rot.Transpose();
Vec2 n = -(RotT * normal);
Vec2 nAbs = Abs(n);
if (nAbs.x > nAbs.y)
{
if (Sign(n.x) > 0.0f)
{
c[0].v.Set(h.x, -h.y);
c[0].fp.e.inEdge2 = EDGE3;
c[0].fp.e.outEdge2 = EDGE4;
c[1].v.Set(h.x, h.y);
c[1].fp.e.inEdge2 = EDGE4;
c[1].fp.e.outEdge2 = EDGE1;
}
else
{
c[0].v.Set(-h.x, h.y);
c[0].fp.e.inEdge2 = EDGE1;
c[0].fp.e.outEdge2 = EDGE2;
c[1].v.Set(-h.x, -h.y);
c[1].fp.e.inEdge2 = EDGE2;
c[1].fp.e.outEdge2 = EDGE3;
}
}
else
{
if (Sign(n.y) > 0.0f)
{
c[0].v.Set(h.x, h.y);
c[0].fp.e.inEdge2 = EDGE4;
c[0].fp.e.outEdge2 = EDGE1;
c[1].v.Set(-h.x, h.y);
c[1].fp.e.inEdge2 = EDGE1;
c[1].fp.e.outEdge2 = EDGE2;
}
else
{
c[0].v.Set(-h.x, -h.y);
c[0].fp.e.inEdge2 = EDGE2;
c[0].fp.e.outEdge2 = EDGE3;
c[1].v.Set(h.x, -h.y);
c[1].fp.e.inEdge2 = EDGE3;
c[1].fp.e.outEdge2 = EDGE4;
}
}
c[0].v = pos + Rot * c[0].v;
c[1].v = pos + Rot * c[1].v;
}
bool IntervalIntersect(const std::vector<float2> &aVertices,
const std::vector<float2> &bVertices, const float2 &axis, const float2 &relPos,
const Mat22 &xOrient, f32 &taxis, f32 tmax)
{
SATProjection proj0 = GetInterval(aVertices, axis * xOrient.Transpose());
SATProjection proj1 = GetInterval(bVertices, axis);
f32 h = relPos.dot(axis);
proj0.min += h;
proj0.max += h;
f32 d0 = proj0.min - proj1.max;
f32 d1 = proj1.min - proj0.max;
if(d0 > 0.0f || d1 > 0.0f)
{
return false;
}
taxis = d0 > d1 ? d0 : d1;
return true;
};
// The normal points from A to B
int Collide(Contact* contacts, Body* bodyA, Body* bodyB)
{
// Setup
Vec2 hA = 0.5f * bodyA->width;
Vec2 hB = 0.5f * bodyB->width;
Vec2 posA = bodyA->position;
Vec2 posB = bodyB->position;
Mat22 RotA(bodyA->rotation), RotB(bodyB->rotation);
Mat22 RotAT = RotA.Transpose();
Mat22 RotBT = RotB.Transpose();
Vec2 a1 = RotA.col1, a2 = RotA.col2;
Vec2 b1 = RotB.col1, b2 = RotB.col2;
Vec2 dp = posB - posA;
Vec2 dA = RotAT * dp;
Vec2 dB = RotBT * dp;
Mat22 C = RotAT * RotB;
Mat22 absC = Abs(C);
Mat22 absCT = absC.Transpose();
// Box A faces
Vec2 faceA = Abs(dA) - hA - absC * hB;
if (faceA.x > 0.0f || faceA.y > 0.0f)
return 0;
// Box B faces
Vec2 faceB = Abs(dB) - absCT * hA - hB;
if (faceB.x > 0.0f || faceB.y > 0.0f)
return 0;
// Find best axis
Axis axis;
float separation;
Vec2 normal;
// Box A faces
axis = FACE_A_X;
separation = faceA.x;
normal = dA.x > 0.0f ? RotA.col1 : -RotA.col1;
const float relativeTol = 0.95f;
const float absoluteTol = 0.01f;
if (faceA.y > relativeTol * separation + absoluteTol * hA.y)
{
axis = FACE_A_Y;
separation = faceA.y;
normal = dA.y > 0.0f ? RotA.col2 : -RotA.col2;
}
// Box B faces
if (faceB.x > relativeTol * separation + absoluteTol * hB.x)
{
axis = FACE_B_X;
separation = faceB.x;
normal = dB.x > 0.0f ? RotB.col1 : -RotB.col1;
}
if (faceB.y > relativeTol * separation + absoluteTol * hB.y)
{
axis = FACE_B_Y;
separation = faceB.y;
normal = dB.y > 0.0f ? RotB.col2 : -RotB.col2;
}
// Setup clipping plane data based on the separating axis
Vec2 frontNormal, sideNormal;
ClipVertex incidentEdge[2];
float front, negSide, posSide;
char negEdge, posEdge;
// Compute the clipping lines and the line segment to be clipped.
switch (axis)
{
case FACE_A_X:
{
frontNormal = normal;
front = Dot(posA, frontNormal) + hA.x;
sideNormal = RotA.col2;
float side = Dot(posA, sideNormal);
negSide = -side + hA.y;
posSide = side + hA.y;
negEdge = EDGE3;
posEdge = EDGE1;
ComputeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal);
}
break;
case FACE_A_Y:
{
frontNormal = normal;
front = Dot(posA, frontNormal) + hA.y;
sideNormal = RotA.col1;
float side = Dot(posA, sideNormal);
negSide = -side + hA.x;
posSide = side + hA.x;
negEdge = EDGE2;
posEdge = EDGE4;
ComputeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal);
}
break;
case FACE_B_X:
{
frontNormal = -normal;
front = Dot(posB, frontNormal) + hB.x;
sideNormal = RotB.col2;
float side = Dot(posB, sideNormal);
negSide = -side + hB.y;
posSide = side + hB.y;
negEdge = EDGE3;
posEdge = EDGE1;
ComputeIncidentEdge(incidentEdge, hA, posA, RotA, frontNormal);
}
break;
case FACE_B_Y:
{
frontNormal = -normal;
front = Dot(posB, frontNormal) + hB.y;
sideNormal = RotB.col1;
float side = Dot(posB, sideNormal);
negSide = -side + hB.x;
posSide = side + hB.x;
negEdge = EDGE2;
posEdge = EDGE4;
ComputeIncidentEdge(incidentEdge, hA, posA, RotA, frontNormal);
}
break;
}
// clip other face with 5 box planes (1 face plane, 4 edge planes)
ClipVertex clipPoints1[2];
ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, negSide, negEdge);
if (np < 2)
return 0;
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, posSide, posEdge);
if (np < 2)
return 0;
// Now clipPoints2 contains the clipping points.
// Due to roundoff, it is possible that clipping removes all points.
int numContacts = 0;
for (int i = 0; i < 2; ++i)
{
float separation = Dot(frontNormal, clipPoints2[i].v) - front;
if (separation <= 0)
{
contacts[numContacts].separation = separation;
contacts[numContacts].normal = normal;
// slide contact point onto reference face (easy to cull)
contacts[numContacts].position = clipPoints2[i].v - separation * frontNormal;
contacts[numContacts].feature = clipPoints2[i].fp;
if (axis == FACE_B_X || axis == FACE_B_Y)
Flip(contacts[numContacts].feature);
++numContacts;
}
}
return numContacts;
}
//Vec2 offset;//((float)(uWidth / 2), (float)(uHeight / 5));
void DrawJoint(Joint* joint)
{
float factor = 10.0f;
iS32 Point[2][2];
//iLog("Start DrawJoint ");
switch ( demoIndex )
{
case 2:
factor = 12.0f;
break;
case 6:
factor = 15.0f;
break;
default:
return;
}
Body* b1 = joint->body1;
Body* b2 = joint->body2;
R1.Mat22_1(b1->rotation);
R2.Mat22_1(b2->rotation);
x1 = b1->position;
p1 = x1 + R1 * joint->localAnchor1;
x2 = b2->position;
p2 = x2 + R2 * joint->localAnchor2;
//#if 1
iU16 *pScrBuffer;
iU16 uWidth=240, uHeight=320;
if( i51AdeMmiGetScreenScale ( &uWidth , &uHeight ) == 0 ) return;
//GetFullScreenBuffer( &pScrBuffer, &uWidth, &uHeight );
offset.Set((float)(uWidth / 2), (float)(uHeight / 5));
x1 *= factor;
p1 *= factor;
x2 *= factor;
p2 *= factor;
x1 += offset;
p1 += offset;
x2 += offset;
p2 += offset;
x1.y = uHeight - x1.y;
p1.y = uHeight - p1.y;
x2.y = uHeight - x2.y;
p2.y = uHeight - p2.y;
// GUI_BEGIN(pScrBuffer, uWidth, uHeight);
// DrawLine((int16)x1.x, (int16)x1.y, (int16)p1.x, (int16)p1.y, MEX_COLOR_RED);
// DrawLine((int16)x2.x, (int16)x2.y, (int16)p2.x, (int16)p2.y, MEX_COLOR_RED);
// GUI_END();
//#endif
Point[0][0] = (iS32)x1.x;
Point[0][1] = (iS32)x1.y;
Point[1][0] = (iS32)p1.x;
Point[1][1] = (iS32)p1.y;
i51KitG2DrawLine(Point, 0XFF00);
Point[0][0] = (iS32)x2.x;
Point[0][1] = (iS32)x2.y;
Point[1][0] = (iS32)p2.x;
Point[1][1] = (iS32)p2.y;
i51KitG2DrawLine(Point, 0XFF00);
//iLog("End DrawJoint ");
}
void DrawBody(Body* body)
{
// uint16 *pScrBuffer;
// uint16 uWidth, uHeight;
// GetFullScreenBuffer( &pScrBuffer, &uWidth, &uHeight );
iU16 *pScrBuffer;
iU16 uWidth=0, uHeight=0;
iS32 Point[2][2];
if( i51AdeMmiGetScreenScale ( &uWidth , &uHeight ) == 0 ) return;
// GetFullScreenBuffer( &pScrBuffer, &uWidth, &uHeight );
//iLog("Start DrawBody ");
R.Mat22_1(body->rotation);
x = body->position;
h = 0.5f * body->width;
offset.Set((float)(uWidth / 2), (float)(uHeight / 5));
v1 = x + R * Vec2(-h.x, -h.y);
v2 = x + R * Vec2( h.x, -h.y);
v3 = x + R * Vec2( h.x, h.y);
v4 = x + R * Vec2(-h.x, h.y);
//#if 1
float factor = 10.0f;
switch ( demoIndex )
{
case 1:
factor = 30.0f;
break;
case 2:
factor = 12.0f;
break;
case 5:
factor = 15.0f;
break;
case 6:
factor = 15.0f;
break;
case 9:
factor = 6.0f;
break;
default:
factor = 10.0f;
break;
}
v1 *= factor;
v2 *= factor;
v3 *= factor;
v4 *= factor;
v1 += offset;
v2 += offset;
v3 += offset;
v4 += offset;
v1.y = uHeight - v1.y;
v2.y = uHeight - v2.y;
v3.y = uHeight - v3.y;
v4.y = uHeight - v4.y;
//log(MEX_LOG_APP0, "(%d, %d), (%d, %d)", (int16)v1.x, (int16)v1.y, (int16)v2.x, (int16)v2.y);
//log(MEX_LOG_APP0, "(%d, %d), (%d, %d)", (int16)v2.x, (int16)v2.y, (int16)v3.x, (int16)v3.y);
//log(MEX_LOG_APP0, "(%d, %d), (%d, %d)", (int16)v3.x, (int16)v3.y, (int16)v4.x, (int16)v4.y);
//log(MEX_LOG_APP0, "(%d, %d), (%d, %d)", (int16)v4.x, (int16)v4.y, (int16)v1.x, (int16)v1.y);
//
// GUI_BEGIN(pScrBuffer, uWidth, uHeight);
// DrawLine((int16)v1.x, (int16)v1.y, (int16)v2.x, (int16)v2.y, MEX_COLOR_WHITE);
// DrawLine((int16)v2.x, (int16)v2.y, (int16)v3.x, (int16)v3.y, MEX_COLOR_WHITE);
// DrawLine((int16)v3.x, (int16)v3.y, (int16)v4.x, (int16)v4.y, MEX_COLOR_WHITE);
// DrawLine((int16)v4.x, (int16)v4.y, (int16)v1.x, (int16)v1.y, MEX_COLOR_WHITE);
// GUI_END();
// GUI_BEGIN(pScrBuffer, uWidth, uHeight);
//#endif
Point[0][0] = (iS32)v1.x;
Point[0][1] = (iS32)v1.y;
Point[1][0] = (iS32)v2.x;
Point[1][1] = (iS32)v2.y;
i51KitG2DrawLine(Point, 0XFF00);
Point[0][0] = (iS32)v2.x;
Point[0][1] = (iS32)v2.y;
Point[1][0] = (iS32)v3.x;
Point[1][1] = (iS32)v3.y;
i51KitG2DrawLine(Point, 0XFF00);
Point[0][0] = (iS32)v3.x;
Point[0][1] = (iS32)v3.y;
Point[1][0] = (iS32)v4.x;
Point[1][1] = (iS32)v4.y;
i51KitG2DrawLine(Point, 0XFF00);
Point[0][0] = (iS32)v4.x;
Point[0][1] = (iS32)v4.y;
Point[1][0] = (iS32)v1.x;
Point[1][1] = (iS32)v1.y;
i51KitG2DrawLine(Point, 0XFF00);
//iLog("End DrawBody ");
}
void MouseJoint::InitVelocityConstraints(const SolverData& data)
{
m_indexB = m_bodyB->m_islandIndex;
m_localCenterB = m_bodyB->m_sweep.localCenter;
m_invMassB = m_bodyB->m_invMass;
m_invIB = m_bodyB->m_invI;
Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
Rot qB(aB);
float32 mass = m_bodyB->GetMass();
// Frequency
float32 omega = 2.0f * pi * m_frequencyHz;
// Damping coefficient
float32 d = 2.0f * mass * m_dampingRatio * omega;
// Spring stiffness
float32 k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
float32 h = data.step.dt;
assert(d + h * k > epsilon);
m_gamma = h * (d + h * k);
if (m_gamma != 0.0f)
{
m_gamma = 1.0f / m_gamma;
}
m_beta = h * k * m_gamma;
// Compute the effective mass matrix.
m_rB = Mul(qB, m_localAnchorB - m_localCenterB);
// K = [(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]
Mat22 K;
K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
K.ex.y = -m_invIB * m_rB.x * m_rB.y;
K.ey.x = K.ex.y;
K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;
m_mass = K.GetInverse();
m_C = cB + m_rB - m_targetA;
m_C *= m_beta;
// Cheat with some damping
wB *= 0.98f;
if (data.step.warmStarting)
{
m_impulse *= data.step.dtRatio;
vB += m_invMassB * m_impulse;
wB += m_invIB * Cross(m_rB, m_impulse);
}
else
{
m_impulse.SetZero();
}
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
