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];
            }
        }