예제 #1
0
        /// <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);
        }
예제 #2
0
        /// <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;
                }
            }
        }
예제 #3
0
        /// <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);
        }