Example #1
0
        private void FourCollisionsButton_Click(object sender, EventArgs e)
        {
            CollisionAPI API = new CollisionAPI();
            // Create some obstacles, at 4 corners of a square
            List <CollisionShape> shapes = new List <CollisionShape>();

            CollisionSphere sphere = new CollisionSphere(new Vector3(0f, 0f, 2f), 1f);

            shapes.Add(sphere);
            API.AddCollisionShape(sphere, 1);

            CollisionCapsule capsule = new CollisionCapsule(new Vector3(10f, 0f, 0f),
                                                            new Vector3(10f, 0f, 4f),
                                                            1f);

            shapes.Add(capsule);
            API.AddCollisionShape(capsule, 2);

            // Now, an AABB
            CollisionAABB aabb = new CollisionAABB(new Vector3(9.5f, 9.5f, 0f),
                                                   new Vector3(10.5f, 10.5f, 4f));

            shapes.Add(aabb);
            API.AddCollisionShape(aabb, 3);

            CollisionOBB obb = new CollisionOBB(new Vector3(0f, 10f, 2),
                                                new Vector3[3] {
                new Vector3(1f, 0f, 0f),
                new Vector3(0f, 1f, 0f),
                new Vector3(0f, 0f, 1f)
            },
                                                new Vector3(1f, 1f, 1f));

            shapes.Add(obb);
            API.AddCollisionShape(obb, 4);

            FileStream   f      = new FileStream("C:\\Junk\\DumpSphereTree.txt", FileMode.Create, FileAccess.Write);
            StreamWriter writer = new StreamWriter(f);

            API.DumpSphereTree(writer);
            API.SphereTreeRoot.VerifySphereTree();

            // Now a moving object capsule in the center of the square
            CollisionCapsule moCap = new CollisionCapsule(new Vector3(5f, 5f, 0f),
                                                          new Vector3(5f, 5f, 4f),
                                                          1f);

            // Remember where the moving object started
            Vector3 start = moCap.center;

            // Move the moving object toward each of the obstacles
            foreach (CollisionShape s in shapes)
            {
                moCap.AddDisplacement(start - moCap.center);
                MoveToObject(writer, API, s, moCap);
            }

            writer.Close();
        }
Example #2
0
 private static void TraceObstacle(CollisionShape obstacle)
 {
     if (obstacle is CollisionOBB)
     {
         CollisionOBB box = (CollisionOBB)obstacle;
         MO.Log(" obstacle top {0} center {1} {2}",
                box.center.y + box.extents[1], box.center, box);
     }
     else
     {
         MO.Log(" obstacle center {0} {1}", obstacle.center, obstacle);
     }
 }
        public void ReadBox(string objectId, Matrix4 transform, XmlNode node, PhysicsData physicsData)
        {
            Quaternion rot          = MathHelpers.GetRotation(transform);
            Matrix4    tmpTransform = rot.Inverse().ToRotationMatrix() * transform;
            Vector3    scaleDelta   = tmpTransform.Scale - Vector3.UnitScale;

            if (scaleDelta.Length > PhysicsSerializer.ScaleEpsilon)
            {
                log.Error("Scale is not currently supported for box shapes");
            }
            Vector3 halfExtents = Vector3.Zero;

            foreach (XmlNode childNode in node.ChildNodes)
            {
                switch (childNode.Name)
                {
                case "half_extents":
                    ReadVector(ref halfExtents, childNode);
                    break;

                default:
                    DebugMessage(childNode);
                    break;
                }
            }
            Vector3 center = unitConversion * transform.Translation;

            halfExtents *= unitConversion;

            CollisionShape collisionShape;
            float          angle = 0;
            Vector3        axis  = Vector3.Zero;

            rot.ToAngleAxis(ref angle, ref axis);
            // I could build AABB shapes for boxes that are aligned, but then
            // I can't drop instances in the world with arbitrary rotations.
            // Instead, just always use an OBB.
            //if (angle < PhysicsSerializer.RotateEpsilon) {
            //    collisionShape = new CollisionAABB(center - halfExtents, center + halfExtents);
            //} else {
            // TODO: I'm not sure I understand the OBB constructor
            Vector3[] axes = new Vector3[3];
            axes[0]        = rot.XAxis;
            axes[1]        = rot.YAxis;
            axes[2]        = rot.ZAxis;
            collisionShape = new CollisionOBB(center, axes, halfExtents);
            physicsData.AddCollisionShape(objectId, collisionShape);
        }
        public XmlNode WriteShape(CollisionShape shape, XmlDocument document)
        {
            XmlNode node = document.CreateElement("shape");


            if (shape is CollisionAABB)
            {
                CollisionAABB aabbShape = shape as CollisionAABB;

                // convert to meters (or other scaled unit) before writing out
                Vector3 center     = shape.center / unitConversion;
                Vector3 halfExtent = (aabbShape.max - aabbShape.center) / unitConversion;

                XmlNode translate = WriteTranslate(center, document);
                node.AppendChild(translate);
                XmlNode boxNode = WriteBox(halfExtent, document);
                node.AppendChild(boxNode);
            }
            else if (shape is CollisionOBB)
            {
                CollisionOBB obbShape = shape as CollisionOBB;

                // convert to meters (or other scaled unit) before writing out
                Vector3 center     = shape.center / unitConversion;
                Vector3 halfExtent = obbShape.extents / unitConversion;

                Quaternion rot = Quaternion.Identity;
                rot.FromAxes(obbShape.axes[0], obbShape.axes[1], obbShape.axes[2]);
                XmlNode rotate = WriteRotate(rot, document);
                node.AppendChild(rotate);
                XmlNode translate = WriteTranslate(center, document);
                node.AppendChild(translate);

                XmlNode boxNode = WriteBox(halfExtent, document);
                node.AppendChild(boxNode);
            }
            else if (shape is CollisionCapsule)
            {
                CollisionCapsule capsuleShape = shape as CollisionCapsule;

                // convert to meters (or other scaled unit) before writing out
                Vector3 center = shape.center / unitConversion;
                float   height = capsuleShape.height / unitConversion;
                float   radius = capsuleShape.capRadius / unitConversion;

                Vector3    halfExtent = capsuleShape.topcenter - capsuleShape.center;
                Quaternion rot        = Vector3.UnitY.GetRotationTo(halfExtent);
                XmlNode    rotate     = WriteRotate(rot, document);
                node.AppendChild(rotate);
                XmlNode translate = WriteTranslate(center, document);
                node.AppendChild(translate);
                XmlNode capsuleNode = WriteCapsule(height, radius, document);
                node.AppendChild(capsuleNode);
            }
            else if (shape is CollisionSphere)
            {
                // convert to meters (or other scaled unit) before writing out
                Vector3 center = shape.center / unitConversion;
                float   radius = shape.radius / unitConversion;

                XmlNode translate = WriteTranslate(center, document);
                node.AppendChild(translate);
                XmlNode sphereNode = WriteSphere(radius, document);
                node.AppendChild(sphereNode);
            }
            else
            {
                log.ErrorFormat("Unsupported collision shape: {0}", shape.GetType());
            }
            return(node);
        }
        // Take a different approach, based on an idea of Robin's:
        // Find the farthest point pair, and use that to identify the
        // triangles with one of the points as a vertex.  Then take
        // the normals of the triangles, adjust to object the
        // right-hand rule, and they are the axes.  Compute the center
        // from the average of the farthest points, and extents by
        // dotting farthest - center with the axes.
        private static CollisionShape ExtractBox(SubMesh subMesh)
        {
            if (DoLog)
            {
                Log("");
                Log(string.Format("Extracting box for submesh {0}", subMesh.Name));
            }
            MeshTriangle[] triangles = ExtractSubmeshTriangles(subMesh);
            int            count     = triangles.Length;

            // Find the two farthest vertices across all triangle
            Vector3[] farthestPoints = new Vector3[2] {
                Vector3.Zero, Vector3.Zero
            };
            float farthestDistanceSquared = 0.0f;

            for (int i = 0; i < count; i++)
            {
                MeshTriangle t1 = triangles[i];
                for (int j = 0; j < 3; j++)
                {
                    Vector3 p1 = t1.vertPos[j];
                    for (int r = i; r < count; r++)
                    {
                        MeshTriangle t2 = triangles[r];
                        for (int s = 0; s < 3; s++)
                        {
                            Vector3 p2   = t2.vertPos[s];
                            Vector3 diff = (p1 - p2);
                            float   d    = diff.LengthSquared;
                            //                          if (DoLog)
                            //                              Log(string.Format(" TriVert {0} {1} {2} / {3} {4} {5} dist {6}", i, j, p1, r, s, p2, d));
                            if (d > farthestDistanceSquared)
                            {
                                if (DoLog)
                                {
                                    Log(string.Format(" Largest! TriVert {0} {1} {2} / {3} {4} {5} dist {6}",
                                                      i, j, p1, r, s, p2, d));
                                }
                                farthestDistanceSquared = d;
                                farthestPoints[0]       = p1;
                                farthestPoints[1]       = p2;
                            }
                        }
                    }
                }
            }
            // The center is the average of the farthest points
            Vector3 center = (farthestPoints[0] + farthestPoints[1]) * 0.5f;

            if (DoLog)
            {
                Log(string.Format("The farthest points are {0} and {1}",
                                  farthestPoints[0], farthestPoints[1]));
                Log(string.Format("The center is {0}", center));
            }
            // Now find the three triangles that have the
            // farthestPoints[0] as a vertex
            Vector3[] axes       = new Vector3[] { Vector3.Zero, Vector3.Zero, Vector3.Zero };
            int       foundCount = 0;

            for (int i = 0; i < count; i++)
            {
                MeshTriangle t = triangles[i];
                for (int j = 0; j < 3; j++)
                {
                    Vector3 p = t.vertPos[j];
                    if ((p - farthestPoints[0]).LengthSquared < geometryEpsilon)
                    {
                        Vector3 side1 = t.vertPos[1] - t.vertPos[0];
                        Vector3 side2 = t.vertPos[2] - t.vertPos[1];
                        Vector3 axis  = side1.Cross(side2).ToNormalized();
                        // Ignore this triangle if his normal matches one we already have
                        bool ignore = false;
                        for (int k = 0; k < foundCount; k++)
                        {
                            if (Math.Abs(axis.Cross(axes[k]).LengthSquared) < geometryEpsilon)
                            {
                                ignore = true;
                                break;
                            }
                        }
                        if (!ignore)
                        {
                            Debug.Assert(foundCount < 3, "Found more than three triangles with distinct normals and vertex = farthest point");
                            axes[foundCount] = axis;
                            foundCount++;
                        }
                    }
                }
            }
            // Put the axes in coordinate order
            for (int i = 0; i < 3; i++)
            {
                float largest      = float.MinValue;
                int   largestIndex = i;
                for (int j = 0; j < 3; j++)
                {
                    float v = Math.Abs(axes[j][i]);
                    if (v > largest)
                    {
                        largestIndex = j;
                        largest      = v;
                    }
                }
                if (largestIndex != i)
                {
                    Vector3 t = axes[i];
                    axes[i]            = axes[largestIndex];
                    axes[largestIndex] = t;
                }
                if (axes[i][i] < 0)
                {
                    axes[i] = -axes[i];
                }
            }

            // Put the axes in right-hand-rule order
            if (axes[0].Cross(axes[1]).Dot(axes[2]) < 0)
            {
                axes[2] = -axes[2];
            }
            Debug.Assert(axes[0].Cross(axes[1]).Dot(axes[2]) > 0,
                         "After swapping axes, still don't obey right-hand rule");
            // The extents are just the abs of the dot products of
            // farthest point minus the center with the axes
            Vector3 f       = farthestPoints[0] - center;
            Vector3 extents = new Vector3(Math.Abs(f.Dot(axes[0])),
                                          Math.Abs(f.Dot(axes[1])),
                                          Math.Abs(f.Dot(axes[2])));

            if (DoLog)
            {
                for (int i = 0; i < 3; i++)
                {
                    Log(string.Format(" axis[{0}] {1}, extent[{2}] {3}", i, axes[i], i, extents[i]));
                }
                int[] sign = new int[3] {
                    0, 0, 0
                };
                for (int i = -1; i < 2; i += 2)
                {
                    sign[0] = i;
                    for (int j = -1; j < 2; j += 2)
                    {
                        sign[1] = j;
                        for (int k = -1; k < 2; k += 2)
                        {
                            sign[2] = k;
                            Vector3 corner = center;
                            for (int a = 0; a < 3; a++)
                            {
                                corner += axes[a] * extents[a] * sign[a];
                            }
                            Log(string.Format(" corner[{0},{1},{2}] = {3}", i, j, k, corner));
                        }
                    }
                }
            }
            // Now, is it an obb or an aabb?  Figure out if the axes
            // point in the same direction as the basis vectors, and
            // if so, the order of the axes
            int[] mapping = new int[3] {
                -1, -1, -1
            };
            int foundMapping = 0;

            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    if (Math.Abs(axes[i].Dot(Primitives.UnitBasisVectors[j]) - 1.0f) < .0001f)
                    {
                        if (DoLog)
                        {
                            Log(string.Format(" foundMapping[{0}], basis vector {1}", i, Primitives.UnitBasisVectors[j]));
                        }
                        mapping[i] = j;
                        foundMapping++;
                        break;
                    }
                }
            }
            CollisionShape shape;

            if (foundMapping == 3)
            {
                // It's an AABB, so build the min and max vectors, in
                // the order that matches the unit basis vector order
                Vector3 min = Vector3.Zero;
                Vector3 max = Vector3.Zero;
                for (int i = 0; i < 3; i++)
                {
                    float e = extents[i];
                    int   j = mapping[i];
                    min[j] = center[j] - e;
                    max[j] = center[j] + e;
                }
                shape = new CollisionAABB(min, max);
            }
            else
            {
                // Return the OBB
                shape = new CollisionOBB(center, axes, extents);
            }
            if (DoLog)
            {
                Log(string.Format("Extraction result: {0}", shape));
            }
            return(shape);
        }
        private static CollisionShape ExtractBox_Old(SubMesh subMesh)
        {
            if (DoLog)
            {
                Log("");
                Log(string.Format("Extracting box for submesh {0}", subMesh.Name));
            }
            MeshTriangle[] triangles = ExtractSubmeshTriangles(subMesh);
            int            count     = triangles.Length;

            Plane[] planesUnsorted = new Plane[6];
            int     planeCount     = 0;

            // Iterate through the triangles.  For each triangle,
            // determine the plane it belongs to, and find the plane
            // in the array of planes, or if it's not found, add it.
            for (int i = 0; i < count; i++)
            {
                MeshTriangle t     = triangles[i];
                bool         found = false;
                Plane        p     = new Plane(t.vertPos[0], t.vertPos[1], t.vertPos[2]);
                NormalizePlane(ref p);
                if (DoLog)
                {
                    Log(string.Format(" {0} => plane {1}", t.ToString(), p.ToString()));
                }
                for (int j = 0; j < planeCount; j++)
                {
                    Plane pj = planesUnsorted[j];
                    if (SamePlane(pj, p))
                    {
                        if (DoLog)
                        {
                            Log(string.Format(" plane {0} same as plane {1}", p.ToString(), pj.ToString()));
                        }
                        found = true;
                        break;
                    }
                }
                if (!found)
                {
                    if (planeCount < 6)
                    {
                        if (DoLog)
                        {
                            Log(string.Format(" plane[{0}] = plane {1}", planeCount, p.ToString()));
                        }
                        planesUnsorted[planeCount++] = p;
                    }
                    else
                    {
                        Debug.Assert(false,
                                     string.Format("In the definition of box {0}, more than 6 planes were found",
                                                   subMesh.Name));
                    }
                }
            }
            Debug.Assert(planeCount == 6,
                         string.Format("In the definition of box {0}, fewer than 6 planes were found",
                                       subMesh.Name));
            // Now recreate the planes array, making sure that
            // opposite faces are 3 planes apart
            Plane[] planes    = new Plane[6];
            bool[]  planeUsed = new bool[6];
            for (int i = 0; i < 6; i++)
            {
                planeUsed[i] = false;
            }
            int planePair = 0;

            for (int i = 0; i < 6; i++)
            {
                if (!planeUsed[i])
                {
                    Plane p1 = planesUnsorted[i];
                    planes[planePair] = p1;
                    planeUsed[i]      = true;
                    for (int j = 0; j < 6; j++)
                    {
                        Plane p2 = planesUnsorted[j];
                        if (!planeUsed[j] && !Orthogonal(p2, p1))
                        {
                            planes[3 + planePair++] = p2;
                            planeUsed[j]            = true;
                            break;
                        }
                    }
                }
            }
            Debug.Assert(planePair == 3, "Didn't find 3 pairs of parallel planes");
            // Make sure that the sequence of planes follows the
            // right-hand rule
            if (planes[0].Normal.Cross(planes[1].Normal).Dot(planes[3].Normal) < 0)
            {
                // Swap the first two plane pairs
                Plane p = planes[0];
                planes[0]     = planes[1];
                planes[1]     = p;
                p             = planes[0 + 3];
                planes[0 + 3] = planes[1 + 3];
                planes[1 + 3] = p;
                Debug.Assert(planes[0].Normal.Cross(planes[1].Normal).Dot(planes[3].Normal) > 0,
                             "Even after swapping, planes don't obey the right-hand rule");
            }
            // Now we have our 6 planes, sorted so that opposite
            // planes are 3 planes apart, and so they obey the
            // right-hand rule.  This guarantees that corners
            // correspond.  Find the 8 intersections that define the
            // corners.
            Vector3[] corners     = new Vector3[8];
            int       cornerCount = 0;

            for (int i = 0; i <= 3; i += 3)
            {
                Plane p1 = planes[i];
                for (int j = 1; j <= 4; j += 3)
                {
                    Plane p2 = planes[j];
                    for (int k = 2; k <= 5; k += 3)
                    {
                        Plane   p3     = planes[k];
                        Vector3 corner = -1 * ((p1.D * (p2.Normal.Cross(p3.Normal)) +
                                                p2.D * (p3.Normal.Cross(p1.Normal)) +
                                                p3.D * (p1.Normal.Cross(p2.Normal))) /
                                               p1.Normal.Dot(p2.Normal.Cross(p3.Normal)));
                        Debug.Assert(cornerCount < 8,
                                     string.Format("In the definition of box {0}, more than 8 corners were found",
                                                   subMesh.Name));
                        if (DoLog)
                        {
                            Log(string.Format("  corner#{0}: {1}", cornerCount, corner.ToString()));
                        }
                        corners[cornerCount++] = corner;
                    }
                }
            }
            Debug.Assert(cornerCount == 8,
                         string.Format("In the definition of box {0}, fewer than 8 corners were found",
                                       subMesh.Name));
            // We know that corners correspond.  Now find the center
            Vector3 center = (corners[0] + corners[7]) / 2;

            Debug.Assert((center - (corners[1] + corners[5]) / 2.0f).Length > geometryEpsilon ||
                         (center - (corners[2] + corners[6]) / 2.0f).Length > geometryEpsilon ||
                         (center - (corners[3] + corners[7]) / 2.0f).Length > geometryEpsilon,
                         string.Format("In the definition of box {0}, center definition {0} is not consistent",
                                       subMesh.Name, center.ToString()));
            // Find the extents
            Vector3 extents = new Vector3(Math.Abs((corners[1] - corners[0]).Length / 2.0f),
                                          Math.Abs((corners[3] - corners[1]).Length / 2.0f),
                                          Math.Abs((corners[4] - corners[0]).Length / 2.0f));

            if (DoLog)
            {
                Log(string.Format(" extents {0}", extents.ToString()));
            }
            // Find the axes
            Vector3[] axes = new Vector3[3] {
                (corners[1] - corners[0]).ToNormalized(),
                (corners[3] - corners[1]).ToNormalized(),
                (corners[4] - corners[0]).ToNormalized()
            };
            if (DoLog)
            {
                for (int i = 0; i < 3; i++)
                {
                    Log(string.Format(" axis[{0}] {1}", i, axes[i]));
                }
            }
            // Now, is it an obb or an aabb?  Figure out if the axes
            // point in the same direction as the basis vectors, and
            // if so, the order of the axes
            int[] mapping = new int[3] {
                -1, -1, -1
            };
            int foundMapping = 0;

            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    if (axes[i].Cross(Primitives.UnitBasisVectors[j]).Length < geometryEpsilon)
                    {
                        mapping[i] = j;
                        foundMapping++;
                        break;
                    }
                }
            }
            CollisionShape shape;

            if (foundMapping == 3)
            {
                // It's an AABB, so build the min and max vectors, in
                // the order that matches the unit basis vector order
                Vector3 min = Vector3.Zero;
                Vector3 max = Vector3.Zero;
                for (int i = 0; i < 3; i++)
                {
                    float e = extents[i];
                    int   j = mapping[i];
                    min[j] = center[j] - e;
                    max[j] = center[j] + e;
                }
                shape = new CollisionAABB(min, max);
            }
            else
            {
                Vector3 ruleTest = axes[0].Cross(axes[1]);
                if (axes[2].Dot(ruleTest) < 0)
                {
                    axes[2] = -1 * axes[2];
                }
                // Return the OBB
                shape = new CollisionOBB(center, axes, extents);
            }
            if (DoLog)
            {
                Log(string.Format("Extraction result: {0}", shape));
            }
            return(shape);
        }