//--------------------------------------------------------------------------------------------------
public static void ComputeReferenceEdgesAndBasis(Vec3 eR, Transform rtx, Vec3 n, int axis, byte[] result, out Mat3 basis, out Vec3 e)
{
basis = new Mat3();
e = new Vec3();
n = Transform.MulT(rtx.rotation, n);
if (axis >= 3)
{
axis -= 3;
}
switch (axis)
{
case 0:
if (n.x > 0)
{
result[0] = 1;
result[1] = 8;
result[2] = 7;
result[3] = 9;
e.Set(eR.y, eR.z, eR.x);
basis.SetRows(rtx.rotation.ey, rtx.rotation.ez, rtx.rotation.ex);
}
else
{
result[0] = 11;
result[1] = 3;
result[2] = 10;
result[3] = 5;
e.Set(eR.z, eR.y, eR.x);
basis.SetRows(rtx.rotation.ez, rtx.rotation.ey, -rtx.rotation.ex);
}
break;
case 1:
if (n.y > 0)
{
result[0] = 0;
result[1] = 1;
result[2] = 2;
result[3] = 3;
e.Set(eR.z, eR.x, eR.y);
basis.SetRows(rtx.rotation.ez, rtx.rotation.ex, rtx.rotation.ey);
}
else
{
result[0] = 4;
result[1] = 5;
result[2] = 6;
result[3] = 7;
e.Set(eR.z, eR.x, eR.y);
basis.SetRows(rtx.rotation.ez, -rtx.rotation.ex, -rtx.rotation.ey);
}
break;
case 2:
if (n.z > 0)
{
result[0] = 11;
result[1] = 4;
result[2] = 8;
result[3] = 0;
e.Set(eR.y, eR.x, eR.z);
basis.SetRows(-rtx.rotation.ey, rtx.rotation.ex, rtx.rotation.ez);
}
else
{
result[0] = 6;
result[1] = 10;
result[2] = 2;
result[3] = 9;
e.Set(eR.y, eR.x, eR.z);
basis.SetRows(-rtx.rotation.ey, -rtx.rotation.ex, -rtx.rotation.ez);
}
break;
}
}
//--------------------------------------------------------------------------------------------------
public static void Zero(Mat3 m)
{
m.Set(0, 0, 0, 0, 0, 0, 0, 0, 0);
}
// Resources:
// http://www.randygaul.net/2014/05/22/deriving-obb-to-obb-intersection-sat/
// https://box2d.googlecode.com/files/GDC2007_ErinCatto.zip
// https://box2d.googlecode.com/files/Box2D_Lite.zip
public static void BoxtoBox(Manifold m, Box a, Box b)
{
Transform atx = a.body.GetTransform();
Transform btx = b.body.GetTransform();
Transform aL = a.local;
Transform bL = b.local;
atx = Transform.Mul(atx, aL);
btx = Transform.Mul(btx, bL);
Vec3 eA = a.e;
Vec3 eB = b.e;
// B's frame input A's space
Mat3 C = Mat3.Transpose(atx.rotation) * btx.rotation;
Mat3 absC = new Mat3();
bool parallel = false;
double kCosTol = 1e-6;
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
double val = Math.Abs(C[i][j]);
var o = absC[i];
o[j] = val;
absC[i] = o;
if (val + kCosTol >= 1)
{
parallel = true;
}
}
}
// Vector from center A to center B input A's space
Vec3 t = Transform.MulT(atx.rotation, btx.position - atx.position);
// Query states
double s;
double aMax = -double.MaxValue;
double bMax = -double.MaxValue;
double eMax = -double.MaxValue;
int aAxis = ~0;
int bAxis = ~0;
int eAxis = ~0;
Vec3 nA = new Vec3();
Vec3 nB = new Vec3();
Vec3 nE = new Vec3();
// Face axis checks
// a's x axis
s = Math.Abs(t.x) - (eA.x + Vec3.Dot(absC.Column0(), eB));
if (TrackFaceAxis(ref aAxis, 0, s, ref aMax, atx.rotation.ex, ref nA))
{
return;
}
// a's y axis
s = Math.Abs(t.y) - (eA.y + Vec3.Dot(absC.Column1(), eB));
if (TrackFaceAxis(ref aAxis, 1, s, ref aMax, atx.rotation.ey, ref nA))
{
return;
}
// a's z axis
s = Math.Abs(t.z) - (eA.z + Vec3.Dot(absC.Column2(), eB));
if (TrackFaceAxis(ref aAxis, 2, s, ref aMax, atx.rotation.ez, ref nA))
{
return;
}
// b's x axis
s = Math.Abs(Vec3.Dot(t, C.ex)) - (eB.x + Vec3.Dot(absC.ex, eA));
if (TrackFaceAxis(ref bAxis, 3, s, ref bMax, btx.rotation.ex, ref nB))
{
return;
}
// b's y axis
s = Math.Abs(Vec3.Dot(t, C.ey)) - (eB.y + Vec3.Dot(absC.ey, eA));
if (TrackFaceAxis(ref bAxis, 4, s, ref bMax, btx.rotation.ey, ref nB))
{
return;
}
// b's z axis
s = Math.Abs(Vec3.Dot(t, C.ez)) - (eB.z + Vec3.Dot(absC.ez, eA));
if (TrackFaceAxis(ref bAxis, 5, s, ref bMax, btx.rotation.ez, ref nB))
{
return;
}
if (!parallel)
{
// Edge axis checks
double rA;
double rB;
// Cross( a.x, b.x )
rA = eA.y * absC[0][2] + eA.z * absC[0][1];
rB = eB.y * absC[2][0] + eB.z * absC[1][0];
s = Math.Abs(t.z * C[0][1] - t.y * C[0][2]) - (rA + rB);
if (TrackEdgeAxis(ref eAxis, 6, s, ref eMax, new Vec3(0, -C[0][2], C[0][1]), ref nE))
{
return;
}
// Cross( a.x, b.y )
rA = eA.y * absC[1][2] + eA.z * absC[1][1];
rB = eB.x * absC[2][0] + eB.z * absC[0][0];
s = Math.Abs(t.z * C[1][1] - t.y * C[1][2]) - (rA + rB);
if (TrackEdgeAxis(ref eAxis, 7, s, ref eMax, new Vec3(0, -C[1][2], C[1][1]), ref nE))
{
return;
}
// Cross( a.x, b.z )
rA = eA.y * absC[2][2] + eA.z * absC[2][1];
rB = eB.x * absC[1][0] + eB.y * absC[0][0];
s = Math.Abs(t.z * C[2][1] - t.y * C[2][2]) - (rA + rB);
if (TrackEdgeAxis(ref eAxis, 8, s, ref eMax, new Vec3(0, -C[2][2], C[2][1]), ref nE))
{
return;
}
// Cross( a.y, b.x )
rA = eA.x * absC[0][2] + eA.z * absC[0][0];
rB = eB.y * absC[2][1] + eB.z * absC[1][1];
s = Math.Abs(t.x * C[0][2] - t.z * C[0][0]) - (rA + rB);
if (TrackEdgeAxis(ref eAxis, 9, s, ref eMax, new Vec3(C[0][2], 0, -C[0][0]), ref nE))
{
return;
}
// Cross( a.y, b.y )
rA = eA.x * absC[1][2] + eA.z * absC[1][0];
rB = eB.x * absC[2][1] + eB.z * absC[0][1];
s = Math.Abs(t.x * C[1][2] - t.z * C[1][0]) - (rA + rB);
if (TrackEdgeAxis(ref eAxis, 10, s, ref eMax, new Vec3(C[1][2], 0, -C[1][0]), ref nE))
{
return;
}
// Cross( a.y, b.z )
rA = eA.x * absC[2][2] + eA.z * absC[2][0];
rB = eB.x * absC[1][1] + eB.y * absC[0][1];
s = Math.Abs(t.x * C[2][2] - t.z * C[2][0]) - (rA + rB);
if (TrackEdgeAxis(ref eAxis, 11, s, ref eMax, new Vec3(C[2][2], 0, -C[2][0]), ref nE))
{
return;
}
// Cross( a.z, b.x )
rA = eA.x * absC[0][1] + eA.y * absC[0][0];
rB = eB.y * absC[2][2] + eB.z * absC[1][2];
s = Math.Abs(t.y * C[0][0] - t.x * C[0][1]) - (rA + rB);
if (TrackEdgeAxis(ref eAxis, 12, s, ref eMax, new Vec3(-C[0][1], C[0][0], 0), ref nE))
{
return;
}
// Cross( a.z, b.y )
rA = eA.x * absC[1][1] + eA.y * absC[1][0];
rB = eB.x * absC[2][2] + eB.z * absC[0][2];
s = Math.Abs(t.y * C[1][0] - t.x * C[1][1]) - (rA + rB);
if (TrackEdgeAxis(ref eAxis, 13, s, ref eMax, new Vec3(-C[1][1], C[1][0], 0), ref nE))
{
return;
}
// Cross( a.z, b.z )
rA = eA.x * absC[2][1] + eA.y * absC[2][0];
rB = eB.x * absC[1][2] + eB.y * absC[0][2];
s = Math.Abs(t.y * C[2][0] - t.x * C[2][1]) - (rA + rB);
if (TrackEdgeAxis(ref eAxis, 14, s, ref eMax, new Vec3(-C[2][1], C[2][0], 0), ref nE))
{
return;
}
}
// Artificial axis bias to improve frame coherence
double kRelTol = 0.95;
double kAbsTol = 0.01;
int axis;
double sMax;
Vec3 n;
double faceMax = Math.Max(aMax, bMax);
if (kRelTol * eMax > faceMax + kAbsTol)
{
axis = eAxis;
sMax = eMax;
n = nE;
}
else
{
if (kRelTol * bMax > aMax + kAbsTol)
{
axis = bAxis;
sMax = bMax;
n = nB;
}
else
{
axis = aAxis;
sMax = aMax;
n = nA;
}
}
if (Vec3.Dot(n, btx.position - atx.position) < 0)
{
n = -n;
}
if (axis < 6)
{
Transform rtx;
Transform itx;
Vec3 eR;
Vec3 eI;
bool flip;
if (axis < 3)
{
rtx = atx;
itx = btx;
eR = eA;
eI = eB;
flip = false;
}
else
{
rtx = btx;
itx = atx;
eR = eB;
eI = eA;
flip = true;
n = -n;
}
// Compute reference and incident edge information necessary for clipping
ComputeIncidentFace(itx, eI, n, incident);
Mat3 basis;
Vec3 e;
ComputeReferenceEdgesAndBasis(eR, rtx, n, axis, clipEdges, out basis, out e);
// Clip the incident face against the reference face side planes
int resultNum;
resultNum = Clip(rtx.position, e, clipEdges, basis, incident, results, depths);
if (resultNum != 0)
{
m.contactCount = resultNum;
m.normal = flip ? -n : n;
for (int i = 0; i < resultNum; ++i)
{
Contact c = m.contacts[i];
FeaturePair pair = results[i].f;
if (flip)
{
Swap(ref pair.inI, ref pair.inR);
Swap(ref pair.outI, ref pair.outR);
}
c.fp = results[i].f;
c.position = results[i].v;
c.penetration = depths[i];
}
}
}
else
{
n = atx.rotation * n;
if (Vec3.Dot(n, btx.position - atx.position) < 0)
{
n = -n;
}
Vec3 PA, QA;
Vec3 PB, QB;
SupportEdge(atx, eA, n, out PA, out QA);
SupportEdge(btx, eB, -n, out PB, out QB);
Vec3 CA, CB;
EdgesContact(out CA, out CB, PA, QA, PB, QB);
m.normal = n;
m.contactCount = 1;
Contact c = m.contacts[0];
FeaturePair pair = new FeaturePair();
pair.key = axis;
c.fp = pair;
c.penetration = sMax;
c.position = (CA + CB) * (0.5);
}
}
//--------------------------------------------------------------------------------------------------
public static Vec3 MulT(Transform tx, Vec3 v)
{
return(Mat3.Transpose(tx.rotation) * (v - tx.position));
}
//--------------------------------------------------------------------------------------------------
public static Vec3 MulT(Mat3 r, Vec3 v)
{
return(Mat3.Transpose(r) * v);
}
//--------------------------------------------------------------------------------------------------
public static Mat3 Mul(Mat3 r, Mat3 q)
{
return(r * q);
}
//--------------------------------------------------------------------------------------------------
public static Vec3 Mul(Mat3 r, Vec3 v)
{
return(r * v);
}
//--------------------------------------------------------------------------------------------------
public static Mat3 MulT(Mat3 r, Mat3 q)
{
return(Mat3.Transpose(r) * q);
}
void CalculateMassData()
{
Mat3 inertia = Mat3.Diagonal(0);
InvInertiaModel = Mat3.Diagonal(0);
InvInertiaWorld = Mat3.Diagonal(0);
InvMass = 0;
Mass = 0;
double mass = 0;
if ((Flags & eStatic) > 0 || (Flags & eKinematic) > 0)
{
Vec3.Identity(ref LocalCenter);
WorldCenter = Tx.position;
return;
}
Vec3 lc = new Vec3();
Vec3.Identity(ref lc);
foreach (var box in Boxes)
{
if (box.density == 0)
{
continue;
}
MassData md;
box.ComputeMass(out md);
mass += md.mass;
inertia += md.inertia;
lc += md.center * md.mass;
}
if (mass > 0)
{
Mass = mass;
InvMass = 1 / mass;
lc *= InvMass;
inertia -= (Mat3.Identity * Vec3.Dot(lc, lc) - Mat3.OuterProduct(lc, lc)) * mass;
InvInertiaModel = Mat3.Inverse(inertia);
if ((Flags & eLockAxisX) > 0)
{
Vec3.Identity(ref InvInertiaModel.ex);
}
if ((Flags & eLockAxisY) > 0)
{
Vec3.Identity(ref InvInertiaModel.ey);
}
if ((Flags & eLockAxisZ) > 0)
{
Vec3.Identity(ref InvInertiaModel.ez);
}
}
else
{
// Force all dynamic bodies to have some mass
InvMass = 1;
InvInertiaModel = Mat3.Diagonal(0);
InvInertiaWorld = Mat3.Diagonal(0);
}
LocalCenter = lc;
WorldCenter = Transform.Mul(Tx, lc);
}
public void Solve()
{
// Apply gravity
// Integrate velocities and create state buffers, calculate world inertia
for (int i = 0; i < Bodies.Count; ++i)
{
Body body = Bodies[i];
VelocityState v = Velocities[i];
if ((body.Flags & BodyFlags.eDynamic) > 0)
{
body.ApplyLinearForce(Gravity * body.GravityScale);
// Calculate world space intertia tensor
Mat3 r = body.Tx.rotation;
body.InvInertiaWorld = r * body.InvInertiaModel * Mat3.Transpose(r);
// Integrate velocity
body.LinearVelocity += (body.Force * body.InvMass) * Dt;
body.AngularVelocity += (body.InvInertiaWorld * body.Torque) * Dt;
// From Box2D!
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Pade approximation:
// v2 = v1 * 1 / (1 + c * dt)
body.LinearVelocity *= 1 / (1 + Dt * body.LinearDamping);
body.AngularVelocity *= 1 / (1 + Dt * body.AngularDamping);
}
Velocities[i] = new VelocityState {
v = body.LinearVelocity, w = body.AngularVelocity
};
}
// Create contact solver, pass in state buffers, create buffers for contacts
// Initialize velocity constraint for normal + friction and warm start
ContactSolver.Initialize(this);
ContactSolver.PreSolve(Dt);
// Solve contacts
for (int i = 0; i < Iterations; ++i)
{
ContactSolver.Solve();
}
ContactSolver.ShutDown();
// Copy back state buffers
// Integrate positions
for (int i = 0; i < Bodies.Count; ++i)
{
Body body = Bodies[i];
VelocityState v = Velocities[i];
if ((body.Flags & BodyFlags.eStatic) > 0)
{
continue;
}
body.LinearVelocity = v.v;
body.AngularVelocity = v.w;
// Integrate position
body.WorldCenter += body.LinearVelocity * Dt;
body.Q.Integrate(body.AngularVelocity, Dt);
body.Q = Quaternion.Normalize(body.Q);
body.Tx.rotation = body.Q.ToMat3();
}
if (AllowSleep)
{
// Find minimum sleep time of the entire island
double minSleepTime = double.MaxValue;
for (int i = 0; i < Bodies.Count; ++i)
{
Body body = Bodies[i];
if ((body.Flags & BodyFlags.eStatic) > 0)
{
continue;
}
double sqrLinVel = Vec3.Dot(body.LinearVelocity, body.LinearVelocity);
double cbAngVel = Vec3.Dot(body.AngularVelocity, body.AngularVelocity);
double linTol = Q3_SLEEP_LINEAR;
double angTol = Q3_SLEEP_ANGULAR;
if (sqrLinVel > linTol || cbAngVel > angTol)
{
minSleepTime = 0;
body.SleepTime = 0;
}
else
{
body.SleepTime += Dt;
minSleepTime = Math.Min(minSleepTime, body.SleepTime);
}
}
// Put entire island to sleep so long as the minimum found sleep time
// is below the threshold. If the minimum sleep time reaches below the
// sleeping threshold, the entire island will be reformed next step
// and sleep test will be tried again.
if (minSleepTime > Q3_SLEEP_TIME)
{
for (int i = 0; i < Bodies.Count; ++i)
{
Bodies[i].SetToSleep();
}
}
}
}
