/// <summary>
/// SqrDistance
/// </summary>
/// <param name="line"></param>
/// <param name="box"></param>
/// <param name="pfLParam"></param>
/// <param name="pfBParam0"></param>
/// <param name="pfBParam1"></param>
/// <param name="pfBParam2"></param>
/// <returns>float</returns>
public static float SqrDistance(Line line, Box box, out float pfLParam,
out float pfBParam0, out float pfBParam1, out float pfBParam2)
{
// compute coordinates of line in box coordinate system
Vector3 diff = line.Origin - box.GetCentre();
Vector3 pnt = new Vector3(Vector3.Dot(diff, box.Orientation.Right),
Vector3.Dot(diff, box.Orientation.Up),
Vector3.Dot(diff, box.Orientation.Backward));
Vector3 kDir = new Vector3(Vector3.Dot(line.Dir, box.Orientation.Right),
Vector3.Dot(line.Dir, box.Orientation.Up),
Vector3.Dot(line.Dir, box.Orientation.Backward));
// Apply reflections so that direction vector has nonnegative components.
bool reflect0 = false;
bool reflect1 = false;
bool reflect2 = false;
pfLParam = 0;
if (kDir.X < 0.0f)
{
pnt.X = -pnt.X;
kDir.X = -kDir.X;
reflect0 = true;
}
if (kDir.Y < 0.0f)
{
pnt.Y = -pnt.Y;
kDir.Y = -kDir.Y;
reflect1 = true;
}
if (kDir.Z < 0.0f)
{
pnt.Z = -pnt.Z;
kDir.Z = -kDir.Z;
reflect2 = true;
}
float sqrDistance = 0.0f;
if (kDir.X > 0.0f)
{
if (kDir.Y > 0.0f)
{
if (kDir.Z > 0.0f)
{
// (+,+,+)
Vector3 kPmE = pnt - box.GetHalfSideLengths();
float prodDxPy = kDir.X * kPmE.Y;
float prodDyPx = kDir.Y * kPmE.X;
float prodDzPx, prodDxPz, prodDzPy, prodDyPz;
if (prodDyPx >= prodDxPy)
{
prodDzPx = kDir.Z * kPmE.X;
prodDxPz = kDir.X * kPmE.Z;
if (prodDzPx >= prodDxPz)
{
//Face(0,1,2)
FaceA(ref pnt, kDir, box, kPmE, out pfLParam, ref sqrDistance);
}
else
{
//Face(2,0,1)
FaceB(ref pnt, kDir, box, kPmE, out pfLParam, ref sqrDistance);
}
}
else
{
prodDzPy = kDir.Z * kPmE.Y;
prodDyPz = kDir.Y * kPmE.Z;
if (prodDzPy >= prodDyPz)
{
//Face(1,2,0)
FaceC(ref pnt, kDir, box, kPmE, out pfLParam, ref sqrDistance);
}
else
{
//Face(2,0,1)
FaceB(ref pnt, kDir, box, kPmE, out pfLParam, ref sqrDistance);
}
}
}
else
{
// (+,+,0)
float pmE0 = pnt.X - box.GetHalfSideLengths().X;
float pmE1 = pnt.Y - box.GetHalfSideLengths().Y;
float prod0 = kDir.Y * pmE0;
float prod1 = kDir.X * pmE1;
float delta, invLSqur, inv;
if (prod0 >= prod1)
{
// line intersects P[i0] = e[i0]
pnt.X = box.GetHalfSideLengths().X;
float ppE1 = pnt.Y + box.GetHalfSideLengths().Y;
delta = prod0 - kDir.X * ppE1;
if (delta >= 0.0f)
{
invLSqur = 1.0f / (kDir.X * kDir.X + kDir.Y * kDir.Y);
sqrDistance += delta * delta * invLSqur;
pnt.Y = -box.GetHalfSideLengths().Y;
pfLParam = -(kDir.X * pmE0 + kDir.Y * ppE1) * invLSqur;
}
else
{
inv = 1.0f / kDir.X;
pnt.Y -= prod0 * inv;
pfLParam = -pmE0 * inv;
}
}
else
{
// line intersects P[i1] = e[i1]
pnt.Y = box.GetHalfSideLengths().Y;
float ppE0 = pnt.X + box.GetHalfSideLengths().X;
delta = prod1 - kDir.Y * ppE0;
if (delta >= 0.0f)
{
invLSqur = 1.0f / (kDir.X * kDir.X + kDir.Y * kDir.Y);
sqrDistance += delta * delta * invLSqur;
pnt.X = -box.GetHalfSideLengths().X;
pfLParam = -(kDir.X * ppE0 + kDir.Y * pmE1) * invLSqur;
}
else
{
inv = 1.0f / kDir.Y;
pnt.X -= prod1 * inv;
pfLParam = -pmE1 * inv;
}
}
if (pnt.Z < -box.GetHalfSideLengths().Z)
{
delta = pnt.Z + box.GetHalfSideLengths().Z;
sqrDistance += delta * delta;
pnt.Z = -box.GetHalfSideLengths().Z;
}
else if (pnt.Z > box.GetHalfSideLengths().Z)
{
delta = pnt.Z - box.GetHalfSideLengths().Z;
sqrDistance += delta * delta;
pnt.Z = box.GetHalfSideLengths().Z;
}
}
}
else
{
if (kDir.Z > 0.0f)
{
// (+,0,+)
float pmE0 = pnt.X - box.GetHalfSideLengths().X;
float pmE1 = pnt.Z - box.GetHalfSideLengths().Z;
float prod0 = kDir.Z * pmE0;
float prod1 = kDir.X * pmE1;
float delta, invLSqur, inv;
if (prod0 >= prod1)
{
// line intersects P[i0] = e[i0]
pnt.X = box.GetHalfSideLengths().X;
float ppE1 = pnt.Z + box.GetHalfSideLengths().Z;
delta = prod0 - kDir.X * ppE1;
if (delta >= 0.0f)
{
invLSqur = 1.0f / (kDir.X * kDir.X + kDir.Z * kDir.Z);
sqrDistance += delta * delta * invLSqur;
pnt.Z = -box.GetHalfSideLengths().Z;
pfLParam = -(kDir.X * pmE0 + kDir.Z * ppE1) * invLSqur;
}
else
{
inv = 1.0f / kDir.X;
pnt.Z -= prod0 * inv;
pfLParam = -pmE0 * inv;
}
}
else
{
// line intersects P[i1] = e[i1]
pnt.Z = box.GetHalfSideLengths().Z;
float ppE0 = pnt.X + box.GetHalfSideLengths().X;
delta = prod1 - kDir.Z * ppE0;
if (delta >= 0.0f)
{
invLSqur = 1.0f / (kDir.X * kDir.X + kDir.Z * kDir.Z);
sqrDistance += delta * delta * invLSqur;
pnt.X = -box.GetHalfSideLengths().X;
pfLParam = -(kDir.X * ppE0 + kDir.Z * pmE1) * invLSqur;
}
else
{
inv = 1.0f / kDir.Z;
pnt.X -= prod1 * inv;
pfLParam = -pmE1 * inv;
}
}
if (pnt.Y < -box.GetHalfSideLengths().Y)
{
delta = pnt.Y + box.GetHalfSideLengths().Y;
sqrDistance += delta * delta;
pnt.Y = -box.GetHalfSideLengths().Y;
}
else if (pnt.Y > box.GetHalfSideLengths().Y)
{
delta = pnt.Y - box.GetHalfSideLengths().Y;
sqrDistance += delta * delta;
pnt.Y = box.GetHalfSideLengths().Y;
}
}
else
{
// (+,0,0)
float pmE0 = pnt.X - box.GetHalfSideLengths().X;
float pmE1 = pnt.Y - box.GetHalfSideLengths().Y;
float prod0 = kDir.Y * pmE0;
float prod1 = kDir.X * pmE1;
float delta, invLSqur, inv;
if (prod0 >= prod1)
{
// line intersects P[i0] = e[i0]
pnt.X = box.GetHalfSideLengths().X;
float ppE1 = pnt.Y + box.GetHalfSideLengths().Y;
delta = prod0 - kDir.X * ppE1;
if (delta >= 0.0f)
{
invLSqur = 1.0f / (kDir.X * kDir.X + kDir.Y * kDir.Y);
sqrDistance += delta * delta * invLSqur;
pnt.Y = -box.GetHalfSideLengths().Y;
pfLParam = -(kDir.X * pmE0 + kDir.Y * ppE1) * invLSqur;
}
else
{
inv = 1.0f / kDir.X;
pnt.Y -= prod0 * inv;
pfLParam = -pmE0 * inv;
}
}
else
{
// line intersects P[i1] = e[i1]
pnt.Y = box.GetHalfSideLengths().Y;
float ppE0 = pnt.X + box.GetHalfSideLengths().X;
delta = prod1 - kDir.Y * ppE0;
if (delta >= 0.0f)
{
invLSqur = 1.0f / (kDir.X * kDir.X + kDir.Y * kDir.Y);
sqrDistance += delta * delta * invLSqur;
pnt.X = -box.GetHalfSideLengths().X;
pfLParam = -(kDir.X * ppE0 + kDir.Y * pmE1) * invLSqur;
}
else
{
inv = 1.0f / kDir.Y;
pnt.X -= prod1 * inv;
pfLParam = -pmE1 * inv;
}
}
if (pnt.Z < -box.GetHalfSideLengths().Z)
{
delta = pnt.Z + box.GetHalfSideLengths().Z;
sqrDistance += delta * delta;
pnt.Z = -box.GetHalfSideLengths().Z;
}
else if (pnt.Z > box.GetHalfSideLengths().Z)
{
delta = pnt.Z - box.GetHalfSideLengths().Z;
sqrDistance += delta * delta;
pnt.Z = box.GetHalfSideLengths().Z;
}
}
}
}
else
{
if (kDir.Y > 0.0f)
{
if (kDir.Z > 0.0f)
{
float pmE0 = pnt.Y - box.GetHalfSideLengths().Y;
float pmE1 = pnt.Z - box.GetHalfSideLengths().Z;
float prod0 = kDir.Z * pmE0;
float prod1 = kDir.Y * pmE1;
float delta, invLSqur, inv;
if (prod0 >= prod1)
{
// line intersects P[i0] = e[i0]
pnt.Y = box.GetHalfSideLengths().Y;
float ppE1 = pnt.Z + box.GetHalfSideLengths().Z;
delta = prod0 - kDir.Y * ppE1;
if (delta >= 0.0f)
{
invLSqur = 1.0f / (kDir.Y * kDir.Y + kDir.Z * kDir.Z);
sqrDistance += delta * delta * invLSqur;
pnt.Z = -box.GetHalfSideLengths().Z;
pfLParam = -(kDir.Y * pmE0 + kDir.Z * ppE1) * invLSqur;
}
else
{
inv = 1.0f / kDir.Y;
pnt.Z -= prod0 * inv;
pfLParam = -pmE0 * inv;
}
}
else
{
// line intersects P[i1] = e[i1]
pnt.Z = box.GetHalfSideLengths().Z;
float ppE0 = pnt.Y + box.GetHalfSideLengths().Y;
delta = prod1 - kDir.Z * ppE0;
if (delta >= 0.0f)
{
invLSqur = 1.0f / (kDir.Y * kDir.Y + kDir.Z * kDir.Z);
sqrDistance += delta * delta * invLSqur;
pnt.Y = -box.GetHalfSideLengths().Y;
pfLParam = -(kDir.Y * ppE0 + kDir.Z * pmE1) * invLSqur;
}
else
{
inv = 1.0f / kDir.Z;
pnt.Y -= prod1 * inv;
pfLParam = -pmE1 * inv;
}
}
if (pnt.X < -box.GetHalfSideLengths().X)
{
delta = pnt.X + box.GetHalfSideLengths().X;
sqrDistance += delta * delta;
pnt.X = -box.GetHalfSideLengths().X;
}
else if (pnt.X > box.GetHalfSideLengths().X)
{
delta = pnt.X - box.GetHalfSideLengths().X;
sqrDistance += delta * delta;
pnt.X = box.GetHalfSideLengths().X;
}
}
else
{
// (0,+,0)
float delta;
pfLParam = (box.GetHalfSideLengths().Y - pnt.Y) / kDir.Y;
pnt.Y = box.GetHalfSideLengths().Y;
if (pnt.X < -box.GetHalfSideLengths().X)
{
delta = pnt.X + box.GetHalfSideLengths().X;
sqrDistance += delta * delta;
pnt.X = -box.GetHalfSideLengths().X;
}
else if (pnt.X > box.GetHalfSideLengths().X)
{
delta = pnt.X - box.GetHalfSideLengths().X;
sqrDistance += delta * delta;
pnt.X = box.GetHalfSideLengths().X;
}
if (pnt.Z < -box.GetHalfSideLengths().Z)
{
delta = pnt.Z + box.GetHalfSideLengths().Z;
sqrDistance += delta * delta;
pnt.Z = -box.GetHalfSideLengths().Z;
}
else if (pnt.Z > box.GetHalfSideLengths().Z)
{
delta = pnt.Z - box.GetHalfSideLengths().Z;
sqrDistance += delta * delta;
pnt.Z = box.GetHalfSideLengths().Z;
}
}
}
else
{
if (kDir.Z > 0.0f)
{
float delta;
pfLParam = (box.GetHalfSideLengths().Z - pnt.Z) / kDir.Z;
pnt.Z = box.GetHalfSideLengths().Z;
if (pnt.X < -box.GetHalfSideLengths().X)
{
delta = pnt.X + box.GetHalfSideLengths().X;
sqrDistance += delta * delta;
pnt.X = -box.GetHalfSideLengths().X;
}
else if (pnt.X > box.GetHalfSideLengths().X)
{
delta = pnt.X - box.GetHalfSideLengths().X;
sqrDistance += delta * delta;
pnt.X = box.GetHalfSideLengths().X;
}
if (pnt.Y < -box.GetHalfSideLengths().Y)
{
delta = pnt.Y + box.GetHalfSideLengths().Y;
sqrDistance += delta * delta;
pnt.Y = -box.GetHalfSideLengths().Y;
}
else if (pnt.Y > box.GetHalfSideLengths().Y)
{
delta = pnt.Y - box.GetHalfSideLengths().Y;
sqrDistance += delta * delta;
pnt.Y = box.GetHalfSideLengths().Y;
}
}
else
{
// (0,0,0)
float delta;
if (pnt.X < -box.GetHalfSideLengths().X)
{
delta = pnt.X + box.GetHalfSideLengths().X;
sqrDistance += delta * delta;
pnt.X = -box.GetHalfSideLengths().X;
}
else if (pnt.X > box.GetHalfSideLengths().X)
{
delta = pnt.X - box.GetHalfSideLengths().X;
sqrDistance += delta * delta;
pnt.X = box.GetHalfSideLengths().X;
}
if (pnt.Y < -box.GetHalfSideLengths().Y)
{
delta = pnt.Y + box.GetHalfSideLengths().Y;
sqrDistance += delta * delta;
pnt.Y = -box.GetHalfSideLengths().Y;
}
else if (pnt.Y > box.GetHalfSideLengths().Y)
{
delta = pnt.Y - box.GetHalfSideLengths().Y;
sqrDistance += delta * delta;
pnt.Y = box.GetHalfSideLengths().Y;
}
if (pnt.Z < -box.GetHalfSideLengths().Z)
{
delta = pnt.Z + box.GetHalfSideLengths().Z;
sqrDistance += delta * delta;
pnt.Z = -box.GetHalfSideLengths().Z;
}
else if (pnt.Z > box.GetHalfSideLengths().Z)
{
delta = pnt.Z - box.GetHalfSideLengths().Z;
sqrDistance += delta * delta;
pnt.Z = box.GetHalfSideLengths().Z;
}
}
}
}
// undo reflections
if (reflect0) pnt.X = -pnt.X;
if (reflect1) pnt.Y = -pnt.Y;
if (reflect2) pnt.Z = -pnt.Z;
pfBParam0 = pnt.X;
pfBParam1 = pnt.Y;
pfBParam2 = pnt.Z;
return MathHelper.Max(sqrDistance, 0.0f);
}
/// <summary>
/// FaceC
/// </summary>
/// <param name="kPnt"></param>
/// <param name="kDir"></param>
/// <param name="rkBox"></param>
/// <param name="kPmE"></param>
/// <param name="pfLParam"></param>
/// <param name="sqrDistance"></param>
private static void FaceC(ref Vector3 kPnt,
Vector3 kDir, Box rkBox,
Vector3 kPmE,out float pfLParam, ref float sqrDistance)
{
// 1,2,0
Vector3 kPpE;
float fLSqr, fInv, fTmp, fParam, fT, fDelta;
kPpE.Z = kPnt.Z + rkBox.GetHalfSideLengths().Z;
kPpE.X = kPnt.X + rkBox.GetHalfSideLengths().X;
if (kDir.Y * kPpE.Z >= kDir.Z * kPmE.Y)
{
if (kDir.Y * kPpE.X >= kDir.X * kPmE.Y)
{
// v.Z >= -e.Z, v.X >= -e.X (distance = 0)
kPnt.Y = rkBox.GetHalfSideLengths().Y;
fInv = 1.0f / kDir.Y;
kPnt.Z -= kDir.Z * kPmE.Y * fInv;
kPnt.X -= kDir.X * kPmE.Y * fInv;
pfLParam = -kPmE.Y * fInv;
}
else
{
// v.Z >= -e.Z, v.X < -e.X
fLSqr = kDir.Y * kDir.Y + kDir.X * kDir.X;
fTmp = fLSqr * kPpE.Z - kDir.Z * (kDir.Y * kPmE.Y +
kDir.X * kPpE.X);
if (fTmp <= 2.0f * fLSqr * rkBox.GetHalfSideLengths().Z)
{
fT = fTmp / fLSqr;
fLSqr += kDir.Z * kDir.Z;
fTmp = kPpE.Z - fT;
fDelta = kDir.Y * kPmE.Y + kDir.Z * fTmp +
kDir.X * kPpE.X;
fParam = -fDelta / fLSqr;
sqrDistance += kPmE.Y * kPmE.Y + fTmp * fTmp +
kPpE.X * kPpE.X + fDelta * fParam;
pfLParam = fParam;
kPnt.Y = rkBox.GetHalfSideLengths().Y;
kPnt.Z = fT - rkBox.GetHalfSideLengths().Z;
kPnt.X = -rkBox.GetHalfSideLengths().X;
}
else
{
fLSqr += kDir.Z * kDir.Z;
fDelta = kDir.Y * kPmE.Y + kDir.Z * kPmE.Z +
kDir.X * kPpE.X;
fParam = -fDelta / fLSqr;
sqrDistance += kPmE.Y * kPmE.Y + kPmE.Z * kPmE.Z +
kPpE.X * kPpE.X + fDelta * fParam;
pfLParam = fParam;
kPnt.Y = rkBox.GetHalfSideLengths().Y;
kPnt.Z = rkBox.GetHalfSideLengths().Z;
kPnt.X = -rkBox.GetHalfSideLengths().X;
}
}
}
else
{
if (kDir.Y * kPpE.X >= kDir.X * kPmE.Y)
{
// v.Z < -e.Z, v.X >= -e.X
fLSqr = kDir.Y * kDir.Y + kDir.Z * kDir.Z;
fTmp = fLSqr * kPpE.X - kDir.X * (kDir.Y * kPmE.Y +
kDir.Z * kPpE.Z);
if (fTmp <= 2.0f * fLSqr * rkBox.GetHalfSideLengths().X)
{
fT = fTmp / fLSqr;
fLSqr += kDir.X * kDir.X;
fTmp = kPpE.X - fT;
fDelta = kDir.Y * kPmE.Y + kDir.Z * kPpE.Z +
kDir.X * fTmp;
fParam = -fDelta / fLSqr;
sqrDistance += kPmE.Y * kPmE.Y + kPpE.Z * kPpE.Z +
fTmp * fTmp + fDelta * fParam;
pfLParam = fParam;
kPnt.Y = rkBox.GetHalfSideLengths().Y;
kPnt.Z = -rkBox.GetHalfSideLengths().Z;
kPnt.X = fT - rkBox.GetHalfSideLengths().X;
}
else
{
fLSqr += kDir.X * kDir.X;
fDelta = kDir.Y * kPmE.Y + kDir.Z * kPpE.Z +
kDir.X * kPmE.X;
fParam = -fDelta / fLSqr;
sqrDistance += kPmE.Y * kPmE.Y + kPpE.Z * kPpE.Z +
kPmE.X * kPmE.X + fDelta * fParam;
pfLParam = fParam;
kPnt.Y = rkBox.GetHalfSideLengths().Y;
kPnt.Z = -rkBox.GetHalfSideLengths().Z;
kPnt.X = rkBox.GetHalfSideLengths().X;
}
}
else
{
// v.Z < -e.Z, v.X < -e.X
fLSqr = kDir.Y * kDir.Y + kDir.X * kDir.X;
fTmp = fLSqr * kPpE.Z - kDir.Z * (kDir.Y * kPmE.Y +
kDir.X * kPpE.X);
if (fTmp >= 0.0f)
{
// v.Z-edge is closest
if (fTmp <= 2.0f * fLSqr * rkBox.GetHalfSideLengths().Z)
{
fT = fTmp / fLSqr;
fLSqr += kDir.Z * kDir.Z;
fTmp = kPpE.Z - fT;
fDelta = kDir.Y * kPmE.Y + kDir.Z * fTmp +
kDir.X * kPpE.X;
fParam = -fDelta / fLSqr;
sqrDistance += kPmE.Y * kPmE.Y + fTmp * fTmp +
kPpE.X * kPpE.X + fDelta * fParam;
pfLParam = fParam;
kPnt.Y = rkBox.GetHalfSideLengths().Y;
kPnt.Z = fT - rkBox.GetHalfSideLengths().Z;
kPnt.X = -rkBox.GetHalfSideLengths().X;
}
else
{
fLSqr += kDir.Z * kDir.Z;
fDelta = kDir.Y * kPmE.Y + kDir.Z * kPmE.Z +
kDir.X * kPpE.X;
fParam = -fDelta / fLSqr;
sqrDistance += kPmE.Y * kPmE.Y + kPmE.Z * kPmE.Z
+ kPpE.X * kPpE.X + fDelta * fParam;
pfLParam = fParam;
kPnt.Y = rkBox.GetHalfSideLengths().Y;
kPnt.Z = rkBox.GetHalfSideLengths().Z;
kPnt.X = -rkBox.GetHalfSideLengths().X;
}
return;
}
fLSqr = kDir.Y * kDir.Y + kDir.Z * kDir.Z;
fTmp = fLSqr * kPpE.X - kDir.X * (kDir.Y * kPmE.Y +
kDir.Z * kPpE.Z);
if (fTmp >= 0.0f)
{
// v.X-edge is closest
if (fTmp <= (2.0f * fLSqr * rkBox.GetHalfSideLengths().X))
{
fT = fTmp / fLSqr;
fLSqr += kDir.X * kDir.X;
fTmp = kPpE.X - fT;
fDelta = kDir.Y * kPmE.Y + kDir.Z * kPpE.Z +
kDir.X * fTmp;
fParam = -fDelta / fLSqr;
sqrDistance += kPmE.Y * kPmE.Y + kPpE.Z * kPpE.Z +
fTmp * fTmp + fDelta * fParam;
pfLParam = fParam;
kPnt.Y = rkBox.GetHalfSideLengths().Y;
kPnt.Z = -rkBox.GetHalfSideLengths().Z;
kPnt.X = fT - rkBox.GetHalfSideLengths().X;
}
else
{
fLSqr += kDir.X * kDir.X;
fDelta = kDir.Y * kPmE.Y + kDir.Z * kPpE.Z +
kDir.X * kPmE.X;
fParam = -fDelta / fLSqr;
sqrDistance += kPmE.Y * kPmE.Y + kPpE.Z * kPpE.Z +
kPmE.X * kPmE.X + fDelta * fParam;
pfLParam = fParam;
kPnt.Y = rkBox.GetHalfSideLengths().Y;
kPnt.Z = -rkBox.GetHalfSideLengths().Z;
kPnt.X = rkBox.GetHalfSideLengths().X;
}
return;
}
// (v.Z,v.X)-corner is closest
fLSqr += kDir.X * kDir.X;
fDelta = kDir.Y * kPmE.Y + kDir.Z * kPpE.Z +
kDir.X * kPpE.X;
fParam = -fDelta / fLSqr;
sqrDistance += kPmE.Y * kPmE.Y + kPpE.Z * kPpE.Z +
kPpE.X * kPpE.X + fDelta * fParam;
pfLParam = fParam;
kPnt.Y = rkBox.GetHalfSideLengths().Y;
kPnt.Z = -rkBox.GetHalfSideLengths().Z;
kPnt.X = -rkBox.GetHalfSideLengths().X;
}
}
}
/// <summary>
/// SqrDistance
/// </summary>
/// <param name="point"></param>
/// <param name="box"></param>
/// <param name="pfBParam0"></param>
/// <param name="pfBParam1"></param>
/// <param name="pfBParam2"></param>
/// <returns>float</returns>
public static float SqrDistance(Vector3 point, Box box,
out float pfBParam0, out float pfBParam1, out float pfBParam2)
{
// compute coordinates of point in box coordinate system
Vector3 kDiff = point - box.GetCentre();
Vector3 kClosest = new Vector3(Vector3.Dot(kDiff, box.Orientation.Right),
Vector3.Dot(kDiff, box.Orientation.Up),
Vector3.Dot(kDiff, box.Orientation.Backward));
// project test point onto box
float fSqrDistance = 0.0f;
float fDelta;
if (kClosest.X < -box.GetHalfSideLengths().X)
{
fDelta = kClosest.X + box.GetHalfSideLengths().X;
fSqrDistance += fDelta * fDelta;
kClosest.X = -box.GetHalfSideLengths().X;
}
else if (kClosest.X > box.GetHalfSideLengths().X)
{
fDelta = kClosest.X - box.GetHalfSideLengths().X;
fSqrDistance += fDelta * fDelta;
kClosest.X = box.GetHalfSideLengths().X;
}
if (kClosest.Y < -box.GetHalfSideLengths().Y)
{
fDelta = kClosest.Y + box.GetHalfSideLengths().Y;
fSqrDistance += fDelta * fDelta;
kClosest.Y = -box.GetHalfSideLengths().Y;
}
else if (kClosest.Y > box.GetHalfSideLengths().Y)
{
fDelta = kClosest.Y - box.GetHalfSideLengths().Y;
fSqrDistance += fDelta * fDelta;
kClosest.Y = box.GetHalfSideLengths().Y;
}
if (kClosest.Z < -box.GetHalfSideLengths().Z)
{
fDelta = kClosest.Z + box.GetHalfSideLengths().Z;
fSqrDistance += fDelta * fDelta;
kClosest.Z = -box.GetHalfSideLengths().Z;
}
else if (kClosest.Z > box.GetHalfSideLengths().Z)
{
fDelta = kClosest.Z - box.GetHalfSideLengths().Z;
fSqrDistance += fDelta * fDelta;
kClosest.Z = box.GetHalfSideLengths().Z;
}
pfBParam0 = kClosest.X;
pfBParam1 = kClosest.Y;
pfBParam2 = kClosest.Z;
return MathHelper.Max(fSqrDistance, 0.0f);
}