public void Render(Transform tx, bool awake, Render render)
{
Transform world = Transform.Mul(tx, local);
Vec3[] vertices = new Vec3[8] {
new Vec3(-e.x, -e.y, -e.z),
new Vec3(-e.x, -e.y, e.z),
new Vec3(-e.x, e.y, -e.z),
new Vec3(-e.x, e.y, e.z),
new Vec3(e.x, -e.y, -e.z),
new Vec3(e.x, -e.y, e.z),
new Vec3(e.x, e.y, -e.z),
new Vec3(e.x, e.y, e.z)
};
for (int i = 0; i < 36; i += 3)
{
Vec3 a = Transform.Mul(world, vertices[kBoxIndices[i]]);
Vec3 b = Transform.Mul(world, vertices[kBoxIndices[i + 1]]);
Vec3 c = Transform.Mul(world, vertices[kBoxIndices[i + 2]]);
Vec3 n = Vec3.Normalize(Vec3.Cross(b - a, c - a));
//render->SetPenColor( 0.2f, 0.4f, 0.7f, 0.5f );
//render->SetPenPosition( a.x, a.y, a.z );
//render->Line( b.x, b.y, b.z );
//render->Line( c.x, c.y, c.z );
//render->Line( a.x, a.y, a.z );
render.SetTriNormal(n.x, n.y, n.z);
render.Triangle(a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z);
}
}
public bool Raycast(Transform tx, RaycastData raycast)
{
Transform world = Transform.Mul(tx, local);
Vec3 d = Transform.MulT(world.rotation, raycast.dir);
Vec3 p = Transform.MulT(world, raycast.start);
double epsilon = 1e-8;
double tmin = 0;
double tmax = raycast.t;
// t = (e[ i ] - p.[ i ]) / d[ i ]
double t0;
double t1;
Vec3 n0 = new Vec3();
for (int i = 0; i < 3; ++i)
{
// Check for ray parallel to and outside of AABB
if (Math.Abs(d[i]) < epsilon)
{
// Detect separating axes
if (p[i] < -e[i] || p[i] > e[i])
{
return(false);
}
}
else
{
double d0 = 1 / d[i];
double s = Math.Sign(d[i]);
double ei = e[i] * s;
Vec3 n = new Vec3(0, 0, 0);
n[i] = -s;
t0 = -(ei + p[i]) * d0;
t1 = (ei - p[i]) * d0;
if (t0 > tmin)
{
n0 = n;
tmin = t0;
}
tmax = Math.Min(tmax, t1);
if (tmin > tmax)
{
return(false);
}
}
}
raycast.normal = Transform.Mul(world.rotation, n0);
raycast.toi = tmin;
return(true);
}
//--------------------------------------------------------------------------------------------------
public static void SupportEdge(Transform tx, Vec3 e, Vec3 n, out Vec3 aresult, out Vec3 bresult)
{
n = Transform.MulT(tx.rotation, n);
Vec3 absN = Vec3.Abs(n);
Vec3 a = new Vec3(), b = new Vec3();
// x > y
if (absN.x > absN.y)
{
// x > y > z
if (absN.y > absN.z)
{
a.Set(e.x, e.y, e.z);
b.Set(e.x, e.y, -e.z);
}
// x > z > y || z > x > y
else
{
a.Set(e.x, e.y, e.z);
b.Set(e.x, -e.y, e.z);
}
}
// y > x
else
{
// y > x > z
if (absN.x > absN.z)
{
a.Set(e.x, e.y, e.z);
b.Set(e.x, e.y, -e.z);
}
// z > y > x || y > z > x
else
{
a.Set(e.x, e.y, e.z);
b.Set(-e.x, e.y, e.z);
}
}
double signx = Sign(n.x);
double signy = Sign(n.y);
double signz = Sign(n.z);
a.x *= signx;
a.y *= signy;
a.z *= signz;
b.x *= signx;
b.y *= signy;
b.z *= signz;
aresult = Transform.Mul(tx, a);
bresult = Transform.Mul(tx, b);
}
//--------------------------------------------------------------------------------------------------
// Resources (also see q3BoxtoBox's resources):
// http://www.randygaul.net/2013/10/27/sutherland-hodgman-clipping/
public static int Clip(Vec3 rPos, Vec3 e, byte[] clipEdges, Mat3 basis, ClipVertex[] incident, ClipVertex[] resultVerts, double[] resultDepths)
{
int inCount = 4;
int resultCount;
for (int i = 0; i < 4; ++i)
{
input[i].v = Transform.MulT(basis, incident[i].v - rPos);
}
resultCount = Orthographic(1, e.x, 0, clipEdges[0], input, inCount, result);
if (resultCount == 0)
{
return(0);
}
inCount = Orthographic(1, e.y, 1, clipEdges[1], result, resultCount, input);
if (inCount == 0)
{
return(0);
}
resultCount = Orthographic(-1, e.x, 0, clipEdges[2], input, inCount, result);
if (resultCount == 0)
{
return(0);
}
inCount = Orthographic(-1, e.y, 1, clipEdges[3], result, resultCount, input);
// Keep incident vertices behind the reference face
resultCount = 0;
for (int i = 0; i < inCount; ++i)
{
double d = input[i].v.z - e.z;
if (d <= 0)
{
resultVerts[resultCount].v = Transform.Mul(basis, input[i].v) + rPos;
resultVerts[resultCount].f = input[i].f;
resultDepths[resultCount++] = d;
}
}
Assert(resultCount <= 8);
return(resultCount);
}
internal void SynchronizeProxies()
{
BroadPhase broadphase = Scene.ContactManager.Broadphase;
Tx.position = WorldCenter - Transform.Mul(Tx.rotation, LocalCenter);
AABB aabb;
Transform tx = Tx;
foreach (var box in Boxes)
{
box.ComputeAABB(tx, out aabb);
broadphase.Update(box.broadPhaseIndex, aabb);
}
}
public void ComputeAABB(Transform tx, out AABB aabb)
{
Transform world = Transform.Mul(tx, local);
Vec3 min = new Vec3(double.MaxValue, double.MaxValue, double.MaxValue);
Vec3 max = new Vec3(-double.MaxValue, -double.MaxValue, -double.MaxValue);
for (int i = 0; i < 8; ++i)
{
var v = Transform.Mul(world, Vec3.Mul(kBoxVertices[i], e));
min = Vec3.Min(min, v);
max = Vec3.Max(max, v);
}
aabb.min = min;
aabb.max = max;
}
public bool TestPoint(Transform tx, Vec3 p)
{
Transform world = Transform.Mul(tx, local);
Vec3 p0 = Transform.MulT(world, p);
for (int i = 0; i < 3; ++i)
{
double d = p0[i];
double ei = e[i];
if (d > ei || d < -ei)
{
return(false);
}
}
return(true);
}
// 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 void ComputeIncidentFace(Transform itx, Vec3 e, Vec3 n, ClipVertex[] result)
{
n = -Transform.MulT(itx.rotation, n);
Vec3 absN = Vec3.Abs(n);
if (absN.x > absN.y && absN.x > absN.z)
{
if (n.x > 0)
{
result[0].v.Set(e.x, e.y, -e.z);
result[1].v.Set(e.x, e.y, e.z);
result[2].v.Set(e.x, -e.y, e.z);
result[3].v.Set(e.x, -e.y, -e.z);
result[0].f.inI = 9;
result[0].f.outI = 1;
result[1].f.inI = 1;
result[1].f.outI = 8;
result[2].f.inI = 8;
result[2].f.outI = 7;
result[3].f.inI = 7;
result[3].f.outI = 9;
}
else
{
result[0].v.Set(-e.x, -e.y, e.z);
result[1].v.Set(-e.x, e.y, e.z);
result[2].v.Set(-e.x, e.y, -e.z);
result[3].v.Set(-e.x, -e.y, -e.z);
result[0].f.inI = 5;
result[0].f.outI = 11;
result[1].f.inI = 11;
result[1].f.outI = 3;
result[2].f.inI = 3;
result[2].f.outI = 10;
result[3].f.inI = 10;
result[3].f.outI = 5;
}
}
else if (absN.y > absN.x && absN.y > absN.z)
{
if (n.y > 0)
{
result[0].v.Set(-e.x, e.y, e.z);
result[1].v.Set(e.x, e.y, e.z);
result[2].v.Set(e.x, e.y, -e.z);
result[3].v.Set(-e.x, e.y, -e.z);
result[0].f.inI = 3;
result[0].f.outI = 0;
result[1].f.inI = 0;
result[1].f.outI = 1;
result[2].f.inI = 1;
result[2].f.outI = 2;
result[3].f.inI = 2;
result[3].f.outI = 3;
}
else
{
result[0].v.Set(e.x, -e.y, e.z);
result[1].v.Set(-e.x, -e.y, e.z);
result[2].v.Set(-e.x, -e.y, -e.z);
result[3].v.Set(e.x, -e.y, -e.z);
result[0].f.inI = 7;
result[0].f.outI = 4;
result[1].f.inI = 4;
result[1].f.outI = 5;
result[2].f.inI = 5;
result[2].f.outI = 6;
result[3].f.inI = 5;
result[3].f.outI = 6;
}
}
else
{
if (n.z > 0)
{
result[0].v.Set(-e.x, e.y, e.z);
result[1].v.Set(-e.x, -e.y, e.z);
result[2].v.Set(e.x, -e.y, e.z);
result[3].v.Set(e.x, e.y, e.z);
result[0].f.inI = 0;
result[0].f.outI = 11;
result[1].f.inI = 11;
result[1].f.outI = 4;
result[2].f.inI = 4;
result[2].f.outI = 8;
result[3].f.inI = 8;
result[3].f.outI = 0;
}
else
{
result[0].v.Set(e.x, -e.y, -e.z);
result[1].v.Set(-e.x, -e.y, -e.z);
result[2].v.Set(-e.x, e.y, -e.z);
result[3].v.Set(e.x, e.y, -e.z);
result[0].f.inI = 9;
result[0].f.outI = 6;
result[1].f.inI = 6;
result[1].f.outI = 10;
result[2].f.inI = 10;
result[2].f.outI = 2;
result[3].f.inI = 2;
result[3].f.outI = 9;
}
}
for (int i = 0; i < 4; ++i)
{
result[i].v = Transform.Mul(itx, result[i].v);
}
}
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 Vec3 GetWorldVector(Vec3 v)
{
return(Transform.Mul(Tx.rotation, v));
}
public Vec3 GetWorldPoint(Vec3 p)
{
return(Transform.Mul(Tx, p));
}
