C# IndexedVector3.Abs方法代码示例

        private static int DBoxBox2(ref IndexedVector3 p1, float[] R1,
        ref IndexedVector3 side1, ref IndexedVector3 p2,
        float[] R2, ref IndexedVector3 side2,
        ref IndexedVector3 normal, ref float depth, ref int return_code,
        int maxc, Object contact, int skip, IDiscreteCollisionDetectorInterfaceResult output)
        {
            //IndexedVector3 centerDifference = IndexedVector3.Zero, ppv = IndexedVector3.Zero;
            float[] normalR = null;
            int normalROffsetResult = 0;


            IndexedVector3 A = side1 * 0.5f;
            IndexedVector3 B = side2 * 0.5f;

            int code;
            bool invert_normal;
            // get vector from centers of box 1 to box 2, relative to box 1
            IndexedVector3 p = p2 - p1;

            IndexedVector3 pp = new IndexedVector3();

            DMULTIPLY1_331(ref pp, R1, ref p);		// get pp = p relative to body 1

            // for all 15 possible separating axes:
            //   * see if the axis separates the boxes. if so, return 0.
            //   * find the depth of the penetration along the separating axis (s2)
            //   * if this is the largest depth so far, record it.
            // the normal vector will be set to the separating axis with the smallest
            // depth. note: normalR is set to point to a column of R1 or R2 if that is
            // the smallest depth normal so far. otherwise normalR is 0 and normalC is
            // set to a vector relative to body 1. invert_normal is 1 if the sign of
            // the normal should be flipped.

            float R11 = DDOT44(R1, 0, R2, 0);
            float R12 = DDOT44(R1, 0, R2, 1);
            float R13 = DDOT44(R1, 0, R2, 2);
            float R21 = DDOT44(R1, 1, R2, 0);
            float R22 = DDOT44(R1, 1, R2, 1);
            float R23 = DDOT44(R1, 1, R2, 2);
            float R31 = DDOT44(R1, 2, R2, 0);
            float R32 = DDOT44(R1, 2, R2, 1);
            float R33 = DDOT44(R1, 2, R2, 2);

            float Q11 = Math.Abs(R11);
            float Q12 = Math.Abs(R12);
            float Q13 = Math.Abs(R13);

            float Q21 = Math.Abs(R21);
            float Q22 = Math.Abs(R22);
            float Q23 = Math.Abs(R23);

            float Q31 = Math.Abs(R31);
            float Q32 = Math.Abs(R32);
            float Q33 = Math.Abs(R33);

            float s = -float.MaxValue;
            invert_normal = false;
            code = 0;

            int normalROffset = 0;
            // separating axis = u1,u2,u3
            if (TST(pp.X, (A.X + B.X * Q11 + B.Y * Q12 + B.Z * Q13), R1, ref normalR, 0, ref normalROffset, 1, ref code, ref s, ref invert_normal)) return 0;
            if (TST(pp.Y, (A.Y + B.X * Q21 + B.Y * Q22 + B.Z * Q23), R1, ref normalR, 1, ref normalROffset, 2, ref code, ref s, ref invert_normal)) return 0;
            if (TST(pp.Z, (A.Z + B.X * Q31 + B.Y * Q32 + B.Z * Q33), R1, ref normalR, 2, ref normalROffset, 3, ref code, ref s, ref invert_normal)) return 0;

            // separating axis = v1,v2,v3
            if (TST(DDOT41(R2, 0, ref p, 0), (A.X * Q11 + A.Y * Q21 + A.Z * Q31 + B.X), R2, ref normalR, 0, ref normalROffset, 4, ref code, ref s, ref invert_normal)) return 0;
            if (TST(DDOT41(R2, 1, ref p, 0), (A.X * Q12 + A.Y * Q22 + A.Z * Q32 + B.Y), R2, ref normalR, 1, ref normalROffset, 5, ref code, ref s, ref invert_normal)) return 0;
            if (TST(DDOT41(R2, 2, ref p, 0), (A.X * Q13 + A.Y * Q23 + A.Z * Q33 + B.Z), R2, ref normalR, 2, ref normalROffset, 6, ref code, ref s, ref invert_normal)) return 0;

            // note: cross product axes need to be scaled when s is computed.
            // normal (n1,n2,n3) is relative to box 1.
            // separating axis = u1 x (v1,v2,v3)
            //private static bool TST2(float expr1,float expr2,ref IndexedVector3 normal, ref IndexedVector3 normalC,int cc,ref int code)

            IndexedVector3 normalC = new IndexedVector3();

            float fudge2 = 1.0e-5f;

            Q11 += fudge2;
            Q12 += fudge2;
            Q13 += fudge2;

            Q21 += fudge2;
            Q22 += fudge2;
            Q23 += fudge2;

            Q31 += fudge2;
            Q32 += fudge2;
            Q33 += fudge2;

            // separating axis = u1 x (v1,v2,v3)
            if (TST2(pp.Z * R21 - pp.Y * R31, (A.Y * Q31 + A.Z * Q21 + B.Y * Q13 + B.Z * Q12), 0, -R31, R21, ref normalC, ref normalR, 7, ref code, ref s, ref invert_normal)) return 0;
            if (TST2(pp.Z * R22 - pp.Y * R32, (A.Y * Q32 + A.Z * Q22 + B.X * Q13 + B.Z * Q11), 0, -R32, R22, ref normalC, ref normalR, 8, ref code, ref s, ref invert_normal)) return 0;
            if (TST2(pp.Z * R23 - pp.Y * R33, (A.Y * Q33 + A.Z * Q23 + B.X * Q12 + B.Y * Q11), 0, -R33, R23, ref normalC, ref normalR, 9, ref code, ref s, ref invert_normal)) return 0;

            // separating axis = u2 x (v1,v2,v3)
            if (TST2(pp.X * R31 - pp.Z * R11, (A.X * Q31 + A.Z * Q11 + B.Y * Q23 + B.Z * Q22), R31, 0, -R11, ref normalC, ref normalR, 10, ref code, ref s, ref invert_normal)) return 0;
            if (TST2(pp.X * R32 - pp.Z * R12, (A.X * Q32 + A.Z * Q12 + B.X * Q23 + B.Z * Q21), R32, 0, -R12, ref normalC, ref normalR, 11, ref code, ref s, ref invert_normal)) return 0;
            if (TST2(pp.X * R33 - pp.Z * R13, (A.X * Q33 + A.Z * Q13 + B.X * Q22 + B.Y * Q21), R33, 0, -R13, ref normalC, ref normalR, 12, ref code, ref s, ref invert_normal)) return 0;
//.........这里部分代码省略.........