Ray OBB intersection not working. Ray not transformed into OBB space correctly?

    public class CD_OrientedBoundingBox
    {
        Vector3 Centre;                                     // OBB centre point
        Quaternion Orientation = Quaternion.Identity;       // Orientation of OBB (Rotation matrix is derived from this)
        Matrix Rotation;                                    // Local space rotation matrix (What angle the box is at)
        Vector3 Extents;                                    // Positive halfwidth extents of the OBB along each axis
        
        // To save memory you can store two Vector3 of the rotation axes
        // Compute the 3rd using Vector3.Cross at collision test time (A 3x3 matrix can then be created from these 3 Vector3's)
        
        public CD_OrientedBoundingBox(Vector3 centre, float width, float height, float depth, float yaw, float pitch, float roll)
        {
            Quaternion additionalRot = Quaternion.CreateFromYawPitchRoll(yaw, pitch, roll);
            Orientation *= additionalRot;
            Rotation = Matrix.CreateFromQuaternion(Orientation);
 
            Centre = centre;
            Extents = new Vector3(width * 0.5f, height * 0.5f, depth * 0.5f);
        }
 
        public CD_OrientedBoundingBox(Vector3 centre, float width, float height, float depth, Matrix rotation)
        {
            Quaternion additionalRot = Quaternion.CreateFromRotationMatrix(rotation);
            Orientation *= additionalRot;
            Rotation = Matrix.CreateFromQuaternion(additionalRot);
 
            Centre = centre;
            Extents = new Vector3(width * 0.5f, height * 0.5f, depth * 0.5f);
        }
 
        // Extents (Local Space - For drawing correctly with rotation)
        public Vector3 Extents_HalfWidth
        {
            get { return Extents; }
        }
 
        /// <summary>
        /// AABB min point contained within AABB (World Space) [Transformed Extents to World Space via World Transform/Transform_World]
        /// </summary>
        public Vector3 MinPoint
        {
            get { return Vector3.Transform(-Extents, Transform_World); }  
        }
 
        /// <summary>
        /// AABB max point contained within AABB (World Space) [Transformed Extents to World Space via World Transform/Transform_World]
        /// </summary>
        public Vector3 MaxPoint
        {
            get { return Vector3.Transform(Extents, Transform_World); }  
        }
 
        public Vector3 Position
        {
            get { return Centre; }
            set { Centre = value; }
        }
 
        public Matrix Rotation_Matrix
        {
            get { return Rotation; }
        }
 
        public Vector3 Axis_X   // X axis rotation
        {
            get { return new Vector3(Rotation.M11, Rotation.M12, Rotation.M13); }
        }
 
        public Vector3 Axis_Y   // Y axis rotation
        {
            get { return new Vector3(Rotation.M21, Rotation.M22, Rotation.M23); }
        }
 
        public Vector3 Axis_Z   // Z axis rotation
        {
            get { return new Vector3(Rotation.M31, Rotation.M32, Rotation.M33); }
        }
 
        /// <summary>
        /// Used for converting body (OBB) space into world space (and vice versa) [Derived from rotation, centre]
        /// </summary>
        public Matrix Transform_World
        {
            get 
            {
                Matrix matrix_World = Rotation;
                matrix_World.Translation = Centre;
                return matrix_World;   
            }
        }

                                              

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
42:
43:
44:
45:
46:
47:
48:
49:
50:
51:
52:
53:
54:
55:
56:
57:
58:
59:
60:
61:
62:
63:
64:
65:
66:
67:
68:
69:
70:
71:
72:
73:
74:
75:
76:
77:
78:
79:
80:
81:
82:
83:
84:
85:
86:
87:
88:
89:
90:

    public class CD_Ray
    {
        public Vector3 Origin;
        public Vector3 Direction;
 
        public CD_Ray(Vector3 origin, Vector3 direction)
        {
            // Some collision detections method might not work with a normalised direction vector
            // Therefore direction is not normalised
            Origin = origin;
            Direction = direction;
        }

                                              

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:

class Obb
{
public:
    Obb(const vector &center, const quaternion &orientation, const vector &extents)
    {
        m_transform = matrix::translate(center) * matrix::from_quaternion(orientation) * matrix::scale(extents[0], extents[1], extents[2]);
        m_transformInv = matrix::scale(1.0f/extents[0], 1.0f/extents[1], 1.0f/extents[2]) * matrix::from_quaternion(quaternion::conjugate(orientation)) * matrix::translate(-center);
    }
 
private:
    matrix m_transform;
    matrix m_transformInv;
};

                                              

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:

        public Vector3 GetNormalFromPoint(Vector3 point)
        {
            Vector3 normal = Vector3.Zero;
            float min = float.MaxValue;
            float distance;
 
            point -= Centre;
 
            distance = Math.Abs(Extents.X - Math.Abs(point.X));
            if (distance < min)
            {
                min = distance;
                normal = Math.Sign(point.X) * Vector3.UnitX;    // Cardinal axis for X
            }
            distance = Math.Abs(Extents.Y - Math.Abs(point.Y));
            if (distance < min)
            {
                min = distance;
                normal = Math.Sign(point.Y) * Vector3.UnitY;    // Cardinal axis for Y
            }
            distance = Math.Abs(Extents.Z - Math.Abs(point.Z));
            if (distance < min)
            {
                min = distance;
                normal = Math.Sign(point.Z) * Vector3.UnitZ;    // Cardinal axis for Z
            }
 
            return normal;
        }

                                              

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:

        private bool IntersectOBB(CD_OrientedBoundingBox oBB, ref Contact contact)
        {
            float t_Min = 0.0f;
 
            // Maximum distance ray can travel (makes it a segment, otherwise it would be infinite)
            float t_Max = float.MaxValue;
 
            float ood = 0.0f;
            float t1 = 0.0f;
            float t2 = 0.0f;
 
            // Transform the ray into OBB space
            Matrix orientation_Inverse = Matrix.Transpose(oBB.Rotation_Matrix);
            Vector3 origin_Projected = Vector3.Transform(Origin, orientation_Inverse);
            Vector3 direction_Projected = Vector3.Transform(Direction, orientation_Inverse);
 
            // For all 3 slabs
            if (Math.Abs(Direction.X) < float.Epsilon)
            {
                // Ray is parallel to slab.  No hit if origin (p in book) not within slab
                if (Origin.X < oBB.MinPoint.X || Origin.X > oBB.MaxPoint.X)
                {
                    return false;
                }
            }
            else
            {
                // Compute intersection t value of ray with near and far plane of slab
                ood = 1.0f / Direction.X;
                t1 = (oBB.MinPoint.X - Origin.X) * ood;
                t2 = (oBB.MaxPoint.X - Origin.X) * ood;
 
                // Make (t1) be intersection with near plane, (t2) with far plane
                if (t1 > t2)
                {
                    Swap(ref t1, ref t2);
                }
 
                // Compute the intersection of slab intersection intervals
                t_Min = Math.Max(t_Min, t1);
                t_Max = Math.Min(t_Max, t2);
 
                // Exit with no collision as soon as slab intersection becomes empty
                if (t_Min > t_Max)
                {
                    return false;
                }
            }
            if (Math.Abs(Direction.Y) < float.Epsilon)
            {
                // Ray is parallel to slab.  No hit if origin (p in book) not within slab
                if (Origin.Y < oBB.MinPoint.Y || Origin.Y > oBB.MaxPoint.Y)
                {
                    return false;
                }
            }
            else
            {
                // Compute intersection t value of ray with near and far plane of slab
                ood = 1.0f / Direction.Y;
                t1 = (oBB.MinPoint.Y - Origin.Y) * ood;
                t2 = (oBB.MaxPoint.Y - Origin.Y) * ood;
 
                // Make (t1) be intersection with near plane, (t2) with far plane
                if (t1 > t2)
                {
                    Swap(ref t1, ref t2);
                }
 
                // Compute the intersection of slab intersection intervals
                t_Min = Math.Max(t_Min, t1);
                t_Max = Math.Min(t_Max, t2);
 
                // Exit with no collision as soon as slab intersection becomes empty
                if (t_Min > t_Max)
                {
                    return false;
                }
            }
            if (Math.Abs(Direction.Z) < float.Epsilon)
            {
                // Ray is parallel to slab.  No hit if origin (p in book) not within slab
                if (Origin.Z < oBB.MinPoint.Z || Origin.Z > oBB.MaxPoint.Z)
                {
                    return false;
                }
            }
            else
            {
                // Compute intersection t value of ray with near and far plane of slab
                ood = 1.0f / Direction.Z;
                t1 = (oBB.MinPoint.Z - Origin.Z) * ood;
                t2 = (oBB.MaxPoint.Z - Origin.Z) * ood;
 
                // Make (t1) be intersection with near plane, (t2) with far plane
                if (t1 > t2)
                {
                    Swap(ref t1, ref t2);
                }
 
                // Compute the intersection of slab intersection intervals
                t_Min = Math.Max(t_Min, t1);
                t_Max = Math.Min(t_Max, t2);
 
                // Exit with no collision as soon as slab intersection becomes empty
                if (t_Min > t_Max)
                {
                    return false;
                }
            }
 
            // Ray intersects all 3 slabs
            // Only update contact parameters if this is the closest intersected so far
            if (contact.KeepClosest(t_Min))
            {
                contact.location = Origin + t_Min * Direction;
                contact.normal = contact.location - oBB.Position;
 
                // Only allow this test to return true if the primitive tested is the closest to the ray origin
                return true;
            }
 
            return false;
        }

                                              

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
42:
43:
44:
45:
46:
47:
48:
49:
50:
51:
52:
53:
54:
55:
56:
57:
58:
59:
60:
61:
62:
63:
64:
65:
66:
67:
68:
69:
70:
71:
72:
73:
74:
75:
76:
77:
78:
79:
80:
81:
82:
83:
84:
85:
86:
87:
88:
89:
90:
91:
92:
93:
94:
95:
96:
97:
98:
99:
100:
101:
102:
103:
104:
105:
106:
107:
108:
109:
110:
111:
112:
113:
114:
115:
116:
117:
118:
119:
120:
121:
122:
123:
124:

        private bool IntersectOBB(CD_OrientedBoundingBox oBB, ref Contact contact)
        {
            float t_Min = 0.0f;
 
            // Maximum distance ray can travel (makes it a segment, otherwise it would be infinite)
            float t_Max = float.MaxValue;
 
            float ood = 0.0f;
            float t1 = 0.0f;
            float t2 = 0.0f;
 
            // Transform the ray into OBB space
            Matrix world_Inverse = Matrix.Invert(oBB.Transform_World);
            Vector3 origin_Projected = Vector3.Transform(Origin, world_Inverse);
            Vector3 direction_Projected = Vector3.Transform(Direction, world_Inverse);
 
            // For all 3 slabs
            if (Math.Abs(direction_Projected.X) < float.Epsilon)
            {
                // Ray is parallel to slab.  No hit if origin (p in book) not within slab
                if (origin_Projected.X < oBB.MinPoint.X || origin_Projected.X > oBB.MaxPoint.X)
                {
                    return false;
                }
            }
            else
            {
                // Compute intersection t value of ray with near and far plane of slab
                ood = 1.0f / direction_Projected.X;
                t1 = (oBB.MinPoint.X - origin_Projected.X) * ood;
                t2 = (oBB.MaxPoint.X - origin_Projected.X) * ood;
 
                // Make (t1) be intersection with near plane, (t2) with far plane
                if (t1 > t2)
                {
                    Swap(ref t1, ref t2);
                }
 
                // Compute the intersection of slab intersection intervals
                t_Min = Math.Max(t_Min, t1);
                t_Max = Math.Min(t_Max, t2);
 
                // Exit with no collision as soon as slab intersection becomes empty
                if (t_Min > t_Max)
                {
                    return false;
                }
            }
            if (Math.Abs(direction_Projected.Y) < float.Epsilon)
            {
                // Ray is parallel to slab.  No hit if origin (p in book) not within slab
                if (origin_Projected.Y < oBB.MinPoint.Y || origin_Projected.Y > oBB.MaxPoint.Y)
                {
                    return false;
                }
            }
            else
            {
                // Compute intersection t value of ray with near and far plane of slab
                ood = 1.0f / direction_Projected.Y;
                t1 = (oBB.MinPoint.Y - origin_Projected.Y) * ood;
                t2 = (oBB.MaxPoint.Y - origin_Projected.Y) * ood;
 
                // Make (t1) be intersection with near plane, (t2) with far plane
                if (t1 > t2)
                {
                    Swap(ref t1, ref t2);
                }
 
                // Compute the intersection of slab intersection intervals
                t_Min = Math.Max(t_Min, t1);
                t_Max = Math.Min(t_Max, t2);
 
                // Exit with no collision as soon as slab intersection becomes empty
                if (t_Min > t_Max)
                {
                    return false;
                }
            }
            if (Math.Abs(direction_Projected.Z) < float.Epsilon)
            {
                // Ray is parallel to slab.  No hit if origin (p in book) not within slab
                if (origin_Projected.Z < oBB.MinPoint.Z || origin_Projected.Z > oBB.MaxPoint.Z)
                {
                    return false;
                }
            }
            else
            {
                // Compute intersection t value of ray with near and far plane of slab
                ood = 1.0f / direction_Projected.Z;
                t1 = (oBB.MinPoint.Z - origin_Projected.Z) * ood;
                t2 = (oBB.MaxPoint.Z - origin_Projected.Z) * ood;
 
                // Make (t1) be intersection with near plane, (t2) with far plane
                if (t1 > t2)
                {
                    Swap(ref t1, ref t2);
                }
 
                // Compute the intersection of slab intersection intervals
                t_Min = Math.Max(t_Min, t1);
                t_Max = Math.Min(t_Max, t2);
 
                // Exit with no collision as soon as slab intersection becomes empty
                if (t_Min > t_Max)
                {
                    return false;
                }
            }
 
            // Ray intersects all 3 slabs
            // Only update contact parameters if this is the closest intersected so far
            if (contact.KeepClosest(t_Min))
            {
                contact.location = Origin + t_Min * direction_Projected;
                contact.normal = contact.location - oBB.Position;
 
                // Only allow this test to return true if the primitive tested is the closest to the ray origin
                return true;
            }
 
            return false;
        }

                                              

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
42:
43:
44:
45:
46:
47:
48:
49:
50:
51:
52:
53:
54:
55:
56:
57:
58:
59:
60:
61:
62:
63:
64:
65:
66:
67:
68:
69:
70:
71:
72:
73:
74:
75:
76:
77:
78:
79:
80:
81:
82:
83:
84:
85:
86:
87:
88:
89:
90:
91:
92:
93:
94:
95:
96:
97:
98:
99:
100:
101:
102:
103:
104:
105:
106:
107:
108:
109:
110:
111:
112:
113:
114:
115:
116:
117:
118:
119:
120:
121:
122:
123:
124:

        private bool IntersectAABB(CD_AxisAlignedBoundingBox aABB, ref Contact contact)
        {
            float t_Min = 0.0f;
 
            // Maximum distance ray can travel (makes it a segment, otherwise it would be infinite)
            float t_Max = float.MaxValue;   
            
            float ood = 0.0f;
            float t1 = 0.0f;
            float t2 = 0.0f;
 
            // For all 3 slabs
            if (Math.Abs(Direction.X) < float.Epsilon)
            {
                // Ray is parallel to slab.  No hit if origin (p in book) not within slab
                if (Origin.X < aABB.MinPoint.X || Origin.X > aABB.MaxPoint.X)
                {
                    return false;
                }
            }
            else
            {
                // Compute intersection t value of ray with near and far plane of slab
                ood = 1.0f / Direction.X;
                t1 = (aABB.MinPoint.X - Origin.X) * ood;
                t2 = (aABB.MaxPoint.X - Origin.X) * ood;
 
                // Make (t1) be intersection with near plane, (t2) with far plane
                if (t1 > t2)
                {
                    Swap(ref t1, ref t2);
                }
 
                // Compute the intersection of slab intersection intervals
                t_Min = Math.Max(t_Min, t1);
                t_Max = Math.Min(t_Max, t2);
 
                // Exit with no collision as soon as slab intersection becomes empty
                if (t_Min > t_Max)
                {
                    return false;
                }
            }
            if (Math.Abs(Direction.Y) < float.Epsilon)
            {
                // Ray is parallel to slab.  No hit if origin (p in book) not within slab
                if (Origin.Y < aABB.MinPoint.Y || Origin.Y > aABB.MaxPoint.Y)
                {
                    return false;
                }
            }
            else
            {
                // Compute intersection t value of ray with near and far plane of slab
                ood = 1.0f / Direction.Y;
                t1 = (aABB.MinPoint.Y - Origin.Y) * ood;
                t2 = (aABB.MaxPoint.Y - Origin.Y) * ood;
 
                // Make (t1) be intersection with near plane, (t2) with far plane
                if (t1 > t2)
                {
                    Swap(ref t1, ref t2);
                }
 
                // Compute the intersection of slab intersection intervals
                t_Min = Math.Max(t_Min, t1);
                t_Max = Math.Min(t_Max, t2);
 
                // Exit with no collision as soon as slab intersection becomes empty
                if (t_Min > t_Max)
                {
                    return false;
                }
            }
            if (Math.Abs(Direction.Z) < float.Epsilon)
            {
                // Ray is parallel to slab.  No hit if origin (p in book) not within slab
                if (Origin.Z < aABB.MinPoint.Z || Origin.Z > aABB.MaxPoint.Z)
                {
                    return false;
                }
            }
            else
            {
                // Compute intersection t value of ray with near and far plane of slab
                ood = 1.0f / Direction.Z;
                t1 = (aABB.MinPoint.Z - Origin.Z) * ood;
                t2 = (aABB.MaxPoint.Z - Origin.Z) * ood;
 
                // Make (t1) be intersection with near plane, (t2) with far plane
                if (t1 > t2)
                {
                    Swap(ref t1, ref t2);
                }
 
                // Compute the intersection of slab intersection intervals
                t_Min = Math.Max(t_Min, t1);
                t_Max = Math.Min(t_Max, t2);
 
                // Exit with no collision as soon as slab intersection becomes empty
                if (t_Min > t_Max)
                {
                    return false;
                }
            }
 
            // Ray intersects all 3 slabs
            // Only update contact parameters if this is the closest intersected so far
            if (contact.KeepClosest(t_Min))
            {
                contact.location = Origin + t_Min * Direction;
                contact.normal = aABB.GetNormalFromPoint(contact.location);
 
                // Only allow this test to return true if the primitive tested is the closest to the ray origin
                return true;
            }
              
            return false;
        }

                                              

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
42:
43:
44:
45:
46:
47:
48:
49:
50:
51:
52:
53:
54:
55:
56:
57:
58:
59:
60:
61:
62:
63:
64:
65:
66:
67:
68:
69:
70:
71:
72:
73:
74:
75:
76:
77:
78:
79:
80:
81:
82:
83:
84:
85:
86:
87:
88:
89:
90:
91:
92:
93:
94:
95:
96:
97:
98:
99:
100:
101:
102:
103:
104:
105:
106:
107:
108:
109:
110:
111:
112:
113:
114:
115:
116:
117:
118:
119:

        private bool IntersectOBB(CD_OrientedBoundingBox oBB, ref Contact contact)
        {
            //Matrix rotation_Inverse = Matrix.Transpose(oBB.Orientation_Matrix);
         
            //Vector3 origin_Projected = Vector3.Transform((Origin - oBB.Centre), rotation_Inverse);
            //Vector3 direction_Projected = origin_Projected + Vector3.Transform(Direction, rotation_Inverse);
 
            // P - C.u0 method
            //Vector3 origin_Projected = Vector3.Zero;
            //origin_Projected.X = Vector3.Dot(Origin - oBB.Centre, oBB.Axis_X);
            //origin_Projected.Y = Vector3.Dot(Origin - oBB.Centre, oBB.Axis_Y);
            //origin_Projected.Z = Vector3.Dot(Origin - oBB.Centre, oBB.Axis_Z);
 
            //Vector3 direction_Projected = Vector3.Zero;
            //origin_Projected.X = origin_Projected.X + Vector3.Dot(Direction, oBB.Axis_X);
            //origin_Projected.Y = origin_Projected.Y + Vector3.Dot(Direction, oBB.Axis_Y);
            //origin_Projected.Z = origin_Projected.Z + Vector3.Dot(Direction, oBB.Axis_Z);
 
            // Transform the ray from world space into OBB space
            Matrix world_Inverse = Matrix.Invert(oBB.Transform_World);
            Vector3 origin_Projected = Vector3.Transform(Origin, world_Inverse);
            Vector3 direction_Projected = Vector3.Transform(Direction, world_Inverse);
            direction_Projected.Normalize();
 
            // Min/Max point in OBB space
            Vector3 oBB_Max = oBB.Extents;
            Vector3 oBB_Min = -oBB.Extents;
            
            float t_Min = 0.0f;
 
            // Maximum distance ray can travel (makes it a segment, otherwise it would be infinite)
            float t_Max = float.MaxValue;
 
            float ood = 0.0f;
            float t1 = 0.0f;
            float t2 = 0.0f;
 
            // For all 3 slabs (x, y, z) [A slab is the space between a pair of parallel planes]
            for (int i = 0; i < 3; i++)
            {
                if (Math.Abs(Vector3Index.Get(ref direction_Projected, i)) < float.Epsilon)
                {
                    // Ray is parallel to slab.  No hit if origin (p in book) not within slab
                    if (Vector3Index.Get(ref origin_Projected, i) < Vector3Index.Get(ref oBB_Min, i) || Vector3Index.Get(ref origin_Projected, i) > Vector3Index.Get(ref oBB_Max, i))
                    {
                        return false;
                    }
                }
                else
                {
                    // Compute intersection t value of ray with near and far plane of slab
                    ood = 1.0f / Vector3Index.Get(ref direction_Projected, i);
                    t1 = (Vector3Index.Get(ref oBB_Min, i) - Vector3Index.Get(ref origin_Projected, i)) * ood;
                    t2 = (Vector3Index.Get(ref oBB_Max, i) - Vector3Index.Get(ref origin_Projected, i)) * ood;
 
                    // Make (t1) be intersection with near plane, (t2) with far plane
                    if (t1 > t2)
                    {
                        Swap(ref t1, ref t2);
                    }
 
                    // Compute the intersection of slab intersection intervals
                    t_Min = Math.Max(t_Min, t1);
                    t_Max = Math.Min(t_Max, t2);
 
                    // Exit with no collision as soon as slab intersection becomes empty
                    if (t_Min > t_Max)
                    {
                        return false;
                    }
                }
            }
 
            // Ray intersects all 3 slabs
            // Only update contact parameters if this is the closest intersected so far
            if (contact.KeepClosest(t_Min))
            {
                contact.location = origin_Projected + t_Min * direction_Projected;
                //contact.normal = aABB.GetNormalFromPoint(contact.location);
 
                // Only allow this test to return true if the primitive tested is the closest to the ray origin
                return true;
            }
 
            return false;
        }

                                              

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
42:
43:
44:
45:
46:
47:
48:
49:
50:
51:
52:
53:
54:
55:
56:
57:
58:
59:
60:
61:
62:
63:
64:
65:
66:
67:
68:
69:
70:
71:
72:
73:
74:
75:
76:
77:
78:
79:
80:
81:
82:
83:
84:
85:
86:

        public CD_OrientedBoundingBox(Vector3 centre, float width, float height, float depth, Matrix rotation)
        {
            Quaternion additionalRot = Quaternion.CreateFromRotationMatrix(rotation);
            Orientation *= additionalRot;
            Rotation = Matrix.CreateFromQuaternion(additionalRot);
 
            Centre = centre;
            Extents = new Vector3(width * 0.5f, height * 0.5f, depth * 0.5f);
        }
 
        /// <summary>
        /// AABB min point contained within AABB (World Space) [Transformed Extents to World Space via World Transform/Transform_World]
        /// </summary>
        public Vector3 MinPoint
        {
            get { return Vector3.Transform(-Extents, Transform_World); }  
        }
 
        /// <summary>
        /// AABB max point contained within AABB (World Space) [Transformed Extents to World Space via World Transform/Transform_World]
        /// </summary>
        public Vector3 MaxPoint
        {
            get { return Vector3.Transform(Extents, Transform_World); }  
        }
 
        public Vector3 Position
        {
            get { return Centre; }
            set { Centre = value; }
        }
 
        public Matrix Orientation_Matrix
        {
            get { return Rotation; }
        }
 
        public Quaternion Orientation_Quaternion
        {
            get { return Orientation; }
        }
 
        /// <summary>
        /// Used for converting body (OBB) space into world space (and vice versa) [Derived from rotation, centre]
        /// </summary>
        public Matrix Transform_World
        {
            get 
            {
                Matrix matrix_World = Rotation;
                matrix_World.Translation = Centre;
                return matrix_World;   
            }
        }

                                              

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
42:
43:
44:
45:
46:
47:
48:
49:
50:
51:
52:
53:
54: