public static bool TestCapsuleOBB(CollisionCapsule a, CollisionOBB b, CollisionParms parms)
{
SegOBBParams obbParms = new SegOBBParams(true);
Segment s = Segment.SegmentFromStartAndEnd(a.bottomcenter, a.topcenter);
if (SqrDistSegOBB(s, b, obbParms) < a.capRadius * a.capRadius) {
// If parms.pfLParam == 0, closest is bottomcenter; if == 1,
// closest is topcenter.
Vector3 d = a.bottomcenter + obbParms.pfLParam * s.direction;
// pfBVec is relative to the center of the box, but in box
// coords.
Vector3 f = b.center;
for (int i=0; i<3; i++)
f += obbParms.pfBVec[i] * b.axes[i];
Vector3 n = f - d;
parms.SetNormPart(n);
parms.SetNormObstacle(-n);
if (MO.DoLog) {
MO.Log(" TestCapsuleOBB: pfLParam {0}, pfBVec {1}", obbParms.pfLParam, obbParms.pfBVec);
MO.Log(" TestCapsuleOBB: d {0}, f {1}", f, d);
MO.Log(" -n {0}", -n);
}
return true;
}
else
return false;
}
public static float SqrDistSegOBB(Segment rkSeg, CollisionOBB rkBox,
SegOBBParams pf)
{
Segment kLine = new Segment(rkSeg.origin, rkSeg.direction);
float fSqrDistance = SqrDistSegOBBInternal(kLine,rkBox,pf);
if (pf.pfLParam >= 0.0f) {
if (pf.pfLParam <= 1.0f) {
return fSqrDistance;
} else {
fSqrDistance = XSqDistPtOBB(rkSeg.origin + rkSeg.direction,rkBox,pf);
if (pf.useIt)
pf.pfLParam = 1.0f;
return fSqrDistance;
}
} else {
fSqrDistance = XSqDistPtOBB(rkSeg.origin,rkBox,pf);
if (pf.useIt)
pf.pfLParam = 0.0f;
return fSqrDistance;
}
}
public static float SqrDistSegOBBInternal(Segment rkLine, CollisionOBB rkBox,
SegOBBParams pf)
{
// compute coordinates of line in box coordinate system
Vector3 kDiff = rkLine.origin - rkBox.center;
Vector3 kPnt = new Vector3(kDiff.Dot(rkBox.axes[0]),
kDiff.Dot(rkBox.axes[1]),
kDiff.Dot(rkBox.axes[2]));
Vector3 kDir = new Vector3(rkLine.direction.Dot(rkBox.axes[0]),
rkLine.direction.Dot(rkBox.axes[1]),
rkLine.direction.Dot(rkBox.axes[2]));
// Apply reflections so that direction vector has nonnegative components.
bool[] bReflect = new bool[3] { false, false, false };
int i;
for (i = 0; i < 3; i++) {
if (kDir[i] < 0.0f) {
kPnt[i] = -kPnt[i];
kDir[i] = -kDir[i];
bReflect[i] = true;
}
}
float fSqrDistance = 0.0f;
if (kDir.x > 0.0f) {
if (kDir.y > 0.0f) {
if (kDir.z > 0.0f) {
// (+,+,+)
CaseNoZeros(ref kPnt,kDir,rkBox,pf,ref fSqrDistance);
} else {
// (+,+,0)
Case0(0,1,2,ref kPnt,kDir,rkBox,pf,ref fSqrDistance);
}
} else {
if (kDir.z > 0.0f) {
// (+,0,+)
Case0(0,2,1,ref kPnt,kDir,rkBox,pf,ref fSqrDistance);
} else {
// (+,0,0)
Case00(0,1,2,ref kPnt,kDir,rkBox,pf,ref fSqrDistance);
}
}
} else {
if (kDir.y > 0.0f) {
if (kDir.z > 0.0f) {
// (0,+,+)
Case0(1,2,0,ref kPnt,kDir,rkBox,pf,ref fSqrDistance);
} else {
// (0,+,0)
Case00(1,0,2,ref kPnt,kDir,rkBox,pf,ref fSqrDistance);
}
} else {
if (kDir.z > 0.0f)
{
// (0,0,+)
Case00(2,0,1,ref kPnt,kDir,rkBox,pf,ref fSqrDistance);
} else {
// (0,0,0)
Case000(ref kPnt,rkBox,ref fSqrDistance);
if (pf.useIt)
pf.pfLParam = 0.0f;
}
}
}
if (pf.useIt) {
// undo reflections
for (i = 0; i < 3; i++) {
if (bReflect[i])
kPnt[i] = -kPnt[i];
}
pf.pfBVec = kPnt;
}
return fSqrDistance;
}
public static void Face(int i0, int i1, int i2, ref Vector3 rkPnt,
Vector3 rkDir, CollisionOBB rkBox,
Vector3 rkPmE, SegOBBParams pf,
ref float rfSqrDistance)
{
Vector3 kPpE = Vector3.Zero;
float fLSqr, fInv, fTmp, fParam, fT, fDelta;
kPpE[i1] = rkPnt[i1] + rkBox.extents[i1];
kPpE[i2] = rkPnt[i2] + rkBox.extents[i2];
if (rkDir[i0]*kPpE[i1] >= rkDir[i1]*rkPmE[i0]) {
if (rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0]) {
// v[i1] >= -e[i1], v[i2] >= -e[i2] (distance = 0)
if (pf.useIt) {
rkPnt[i0] = rkBox.extents[i0];
fInv = 1.0f/rkDir[i0];
rkPnt[i1] -= rkDir[i1]*rkPmE[i0]*fInv;
rkPnt[i2] -= rkDir[i2]*rkPmE[i0]*fInv;
pf.pfLParam = -rkPmE[i0]*fInv;
}
} else {
// v[i1] >= -e[i1], v[i2] < -e[i2]
fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i2]*rkDir[i2];
fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] +
rkDir[i2]*kPpE[i2]);
if (fTmp <= 2.0*fLSqr*rkBox.extents[i1]) {
fT = fTmp/fLSqr;
fLSqr += rkDir[i1]*rkDir[i1];
fTmp = kPpE[i1] - fT;
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp +
rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp +
kPpE[i2]*kPpE[i2] + fDelta*fParam;
if (pf.useIt) {
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = fT - rkBox.extents[i1];
rkPnt[i2] = -rkBox.extents[i2];
}
} else {
fLSqr += rkDir[i1]*rkDir[i1];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] +
rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] +
kPpE[i2]*kPpE[i2] + fDelta*fParam;
if (pf.useIt) {
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = rkBox.extents[i1];
rkPnt[i2] = -rkBox.extents[i2];
}
}
}
} else {
if (rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0]) {
// v[i1] < -e[i1], v[i2] >= -e[i2]
fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] +
rkDir[i1]*kPpE[i1]);
if (fTmp <= 2.0*fLSqr*rkBox.extents[i2]) {
fT = fTmp/fLSqr;
fLSqr += rkDir[i2]*rkDir[i2];
fTmp = kPpE[i2] - fT;
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] +
rkDir[i2]*fTmp;
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] +
fTmp*fTmp + fDelta*fParam;
if (pf.useIt) {
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = -rkBox.extents[i1];
rkPnt[i2] = fT - rkBox.extents[i2];
}
} else {
fLSqr += rkDir[i2]*rkDir[i2];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] +
rkDir[i2]*rkPmE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] +
rkPmE[i2]*rkPmE[i2] + fDelta*fParam;
if (pf.useIt) {
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = -rkBox.extents[i1];
rkPnt[i2] = rkBox.extents[i2];
}
}
} else {
// v[i1] < -e[i1], v[i2] < -e[i2]
fLSqr = rkDir[i0]*rkDir[i0]+rkDir[i2]*rkDir[i2];
fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] +
rkDir[i2]*kPpE[i2]);
if (fTmp >= 0.0f) {
// v[i1]-edge is closest
if (fTmp <= 2.0*fLSqr*rkBox.extents[i1])
{
fT = fTmp/fLSqr;
fLSqr += rkDir[i1]*rkDir[i1];
fTmp = kPpE[i1] - fT;
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp +
rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp +
kPpE[i2]*kPpE[i2] + fDelta*fParam;
if (pf.useIt)
{
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = fT - rkBox.extents[i1];
rkPnt[i2] = -rkBox.extents[i2];
}
} else {
fLSqr += rkDir[i1]*rkDir[i1];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] +
rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1]
+ kPpE[i2]*kPpE[i2] + fDelta*fParam;
if (pf.useIt) {
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = rkBox.extents[i1];
rkPnt[i2] = -rkBox.extents[i2];
}
}
return;
}
fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] +
rkDir[i1]*kPpE[i1]);
if (fTmp >= 0.0f) {
// v[i2]-edge is closest
if (fTmp <= 2.0*fLSqr*rkBox.extents[i2]) {
fT = fTmp/fLSqr;
fLSqr += rkDir[i2]*rkDir[i2];
fTmp = kPpE[i2] - fT;
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] +
rkDir[i2]*fTmp;
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] +
fTmp*fTmp + fDelta*fParam;
if (pf.useIt) {
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = -rkBox.extents[i1];
rkPnt[i2] = fT - rkBox.extents[i2];
}
} else {
fLSqr += rkDir[i2]*rkDir[i2];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] +
rkDir[i2]*rkPmE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] +
rkPmE[i2]*rkPmE[i2] + fDelta*fParam;
if (pf.useIt) {
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = -rkBox.extents[i1];
rkPnt[i2] = rkBox.extents[i2];
}
}
return;
}
// (v[i1],v[i2])-corner is closest
fLSqr += rkDir[i2]*rkDir[i2];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] +
rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] +
kPpE[i2]*kPpE[i2] + fDelta*fParam;
if (pf.useIt) {
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = -rkBox.extents[i1];
rkPnt[i2] = -rkBox.extents[i2];
}
}
}
}
public static void CaseNoZeros(ref Vector3 rkPnt, Vector3 rkDir,
CollisionOBB rkBox, SegOBBParams pf,
ref float rfSqrDistance)
{
Vector3 kPmE = new Vector3(rkPnt.x - rkBox.extents[0],
rkPnt.y - rkBox.extents[1],
rkPnt.z - rkBox.extents[2]);
float fProdDxPy, fProdDyPx, fProdDzPx, fProdDxPz, fProdDzPy, fProdDyPz;
fProdDxPy = rkDir.x*kPmE.y;
fProdDyPx = rkDir.y*kPmE.x;
if (fProdDyPx >= fProdDxPy) {
fProdDzPx = rkDir.z*kPmE.x;
fProdDxPz = rkDir.x*kPmE.z;
if (fProdDzPx >= fProdDxPz) {
// line intersects x = e0
Face(0,1,2,ref rkPnt,rkDir,rkBox,kPmE,pf,ref rfSqrDistance);
} else {
// line intersects z = e2
Face(2,0,1,ref rkPnt,rkDir,rkBox,kPmE,pf,ref rfSqrDistance);
}
} else {
fProdDzPy = rkDir.z*kPmE.y;
fProdDyPz = rkDir.y*kPmE.z;
if (fProdDzPy >= fProdDyPz)
{
// line intersects y = e1
Face(1,2,0,ref rkPnt,rkDir,rkBox,kPmE,pf,ref rfSqrDistance);
} else {
// line intersects z = e2
Face(2,0,1,ref rkPnt,rkDir,rkBox,kPmE,pf,ref rfSqrDistance);
}
}
}
public static void Case00(int i0, int i1, int i2, ref Vector3 rkPnt,
Vector3 rkDir, CollisionOBB rkBox,
SegOBBParams pf, ref float rfSqrDistance)
{
float fDelta;
if (pf.useIt)
pf.pfLParam = (rkBox.extents[i0] - rkPnt[i0])/rkDir[i0];
rkPnt[i0] = rkBox.extents[i0];
if (rkPnt[i1] < -rkBox.extents[i1]) {
fDelta = rkPnt[i1] + rkBox.extents[i1];
rfSqrDistance += fDelta*fDelta;
rkPnt[i1] = -rkBox.extents[i1];
}
else if (rkPnt[i1] > rkBox.extents[i1]) {
fDelta = rkPnt[i1] - rkBox.extents[i1];
rfSqrDistance += fDelta*fDelta;
rkPnt[i1] = rkBox.extents[i1];
}
if (rkPnt[i2] < -rkBox.extents[i2]) {
fDelta = rkPnt[i2] + rkBox.extents[i2];
rfSqrDistance += fDelta*fDelta;
rkPnt[i2] = -rkBox.extents[i2];
}
else if (rkPnt[i2] > rkBox.extents[i2]) {
fDelta = rkPnt[i2] - rkBox.extents[i2];
rfSqrDistance += fDelta*fDelta;
rkPnt[i2] = rkBox.extents[i2];
}
}
public static float XSqDistPtOBB(Vector3 rkPoint, CollisionOBB rkBox,
SegOBBParams pf)
{
// compute coordinates of point in box coordinate system
Vector3 kDiff = rkPoint - rkBox.center;
Vector3 kClosest = new Vector3(kDiff.Dot(rkBox.axes[0]),
kDiff.Dot(rkBox.axes[1]),
kDiff.Dot(rkBox.axes[2]));
// project test point onto box
float fSqrDistance = 0.0f;
float fDelta;
for (int i=0; i<3; i++) {
if (kClosest[i] < -rkBox.extents[i]) {
fDelta = kClosest[i] + rkBox.extents[i];
fSqrDistance += fDelta*fDelta;
kClosest[i] = -rkBox.extents[i];
}
else if (kClosest[i] > rkBox.extents[i]) {
fDelta = kClosest[i] - rkBox.extents[i];
fSqrDistance += fDelta*fDelta;
kClosest[i] = rkBox.extents[i];
}
}
if (pf.useIt) {
pf.pfBVec = kClosest;
}
return fSqrDistance;
}
public static void Case0(int i0, int i1, int i2, ref Vector3 rkPnt,
Vector3 rkDir, CollisionOBB rkBox,
SegOBBParams pf, ref float rfSqrDistance)
{
float fPmE0 = rkPnt[i0] - rkBox.extents[i0];
float fPmE1 = rkPnt[i1] - rkBox.extents[i1];
float fProd0 = rkDir[i1]*fPmE0;
float fProd1 = rkDir[i0]*fPmE1;
float fDelta, fInvLSqr, fInv;
if (fProd0 >= fProd1) {
// line intersects P[i0] = e[i0]
rkPnt[i0] = rkBox.extents[i0];
float fPpE1 = rkPnt[i1] + rkBox.extents[i1];
fDelta = fProd0 - rkDir[i0]*fPpE1;
if (fDelta >= 0.0f)
{
fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]);
rfSqrDistance += fDelta*fDelta*fInvLSqr;
if (pf.useIt) {
rkPnt[i1] = -rkBox.extents[i1];
pf.pfLParam = -(rkDir[i0]*fPmE0+rkDir[i1]*fPpE1)*fInvLSqr;
}
} else {
if (pf.useIt) {
fInv = 1.0f/rkDir[i0];
rkPnt[i1] -= fProd0*fInv;
pf.pfLParam = -fPmE0*fInv;
}
}
} else {
// line intersects P[i1] = e[i1]
rkPnt[i1] = rkBox.extents[i1];
float fPpE0 = rkPnt[i0] + rkBox.extents[i0];
fDelta = fProd1 - rkDir[i1]*fPpE0;
if (fDelta >= 0.0f) {
fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]);
rfSqrDistance += fDelta*fDelta*fInvLSqr;
if (pf.useIt) {
rkPnt[i0] = -rkBox.extents[i0];
pf.pfLParam = -(rkDir[i0]*fPpE0+rkDir[i1]*fPmE1)*fInvLSqr;
}
} else {
if (pf.useIt) {
fInv = 1.0f/rkDir[i1];
rkPnt[i0] -= fProd1*fInv;
pf.pfLParam = -fPmE1*fInv;
}
}
}
if (rkPnt[i2] < -rkBox.extents[i2]) {
fDelta = rkPnt[i2] + rkBox.extents[i2];
rfSqrDistance += fDelta*fDelta;
rkPnt[i2] = -rkBox.extents[i2];
}
else if (rkPnt[i2] > rkBox.extents[i2]) {
fDelta = rkPnt[i2] - rkBox.extents[i2];
rfSqrDistance += fDelta*fDelta;
rkPnt[i2] = rkBox.extents[i2];
}
}