コード例 #1
0
        /// <summary>
        /// Inverses the direction of a vector.
        /// </summary>
        /// <param name="value">The vector to inverse.</param>
        /// <returns>The negated vector.</returns>
        public static JVector Negate(JVector value)
        {
            JVector result;

            JVector.Negate(ref value, out result);
            return(result);
        }
コード例 #2
0
        //object locker = new object();
        private void CollisionDetected(RigidBody body1, RigidBody body2, JVector point1, JVector point2, JVector normal,
                                       float penetration)
        {
            Arbiter arbiter;

            arbiterMap.LookUpArbiter(body1, body2, out arbiter);
            if (arbiter == null)
            {
                arbiter = Arbiter.Pool.GetNew();
                arbiter.body1 = body1;
                arbiter.body2 = body2;

                arbiterMap.Add(new ArbiterKey(body1, body2), arbiter);

                addedArbiterQueue.Enqueue(arbiter);

                events.RaiseBodiesBeginCollide(body1, body2);
            }


            Contact contact;

            if (arbiter.body1 == body1)
            {
                JVector.Negate(ref normal, out normal);
                contact = arbiter.AddContact(point1, point2, normal, penetration, contactSettings);
            }
            else
            {
                contact = arbiter.AddContact(point2, point1, normal, penetration, contactSettings);
            }

            if (contact != null) events.RaiseContactCreated(contact);
        }
コード例 #3
0
ファイル: BoxShape.cs プロジェクト: Avatarchik/JPhysics
        /// <summary>
        /// Gets the axis aligned bounding box of the orientated shape.
        /// </summary>
        /// <param name="orientation">The orientation of the shape.</param>
        /// <param name="box">The axis aligned bounding box of the shape.</param>
        public override void GetBoundingBox(ref JMatrix orientation, out JBBox box)
        {
            JMatrix abs; JMath.Absolute(ref orientation, out abs);
            JVector temp;

            JVector.Transform(ref halfSize, ref abs, out temp);

            box.Max = temp;
            JVector.Negate(ref temp, out box.Min);
        }
コード例 #4
0
        /// <summary>
        /// Discrete Circle vs Circle test. Very fast. Generates contact info.
        /// NOTE: check distance for collisions. If negative then a collision has occurred.
        /// This is done to remove all branches from this test and leave it to the user to decide when to branch.
        /// </summary>
        public static void CircleCircleTest(JVector centerA, float radiusA, JVector centerB, float radiusB, out JVector pointA, out JVector pointB, out JVector normal, out float distance)
        {
            // ||A-B|| - (r1+r2) < 0
            float d = JVector.DistanceSquared(centerA, centerB);
            float r = (radiusA + radiusB);

            r *= r;

            distance = d - r;

            normal = (centerA - centerB) / d;
            normal.Normalize();

            // calculate closest 2 points
            pointA = JVector.Negate(normal) * radiusA + centerA;
            pointB = normal * radiusB + centerB;
        }
コード例 #5
0
        public static bool OriginInTriangle(JVector a, JVector b, JVector c)
        {
            float pab = JVector.Cross(JVector.Negate(a), b - a);
            float pbc = JVector.Cross(JVector.Negate(b), c - b);

            if (!SameSign(pab, pbc))
            {
                return(false);
            }

            float pca = JVector.Cross(JVector.Negate(c), a - c);

            if (!SameSign(pab, pca))
            {
                return(false);
            }

            return(true);
        }
コード例 #6
0
        private void FindSupportPoints(RigidBody body1, RigidBody body2,
                                       Shape shape1, Shape shape2, ref JVector point, ref JVector normal,
                                       out JVector point1, out JVector point2)
        {
            JVector mn; JVector.Negate(ref normal, out mn);

            JVector sA; SupportMapping(body1, shape1, ref mn, out sA);
            JVector sB; SupportMapping(body2, shape2, ref normal, out sB);

            JVector.Subtract(ref sA, ref point, out sA);
            JVector.Subtract(ref sB, ref point, out sB);

            float dot1 = JVector.Dot(ref sA, ref normal);
            float dot2 = JVector.Dot(ref sB, ref normal);

            JVector.Multiply(ref normal, dot1, out sA);
            JVector.Multiply(ref normal, dot2, out sB);

            JVector.Add(ref point, ref sA, out point1);
            JVector.Add(ref point, ref sB, out point2);
        }
コード例 #7
0
        internal static bool CircleCapsuleTest(JVector centerA, float radiusA, JVector centerB, JVector axis, float length, float radiusB, out JVector pointA, out JVector pointB, out JVector normal, out float distance)
        {
            // get capsule endpoints
            var p0 = centerB - axis * (length * 0.5f);
            var p1 = centerB + axis * (length * 0.5f);

            // get vector from endpoint to circle
            var D = centerA - p0;

            // project vector onto axis and clamp
            var d = JVector.Dot(D, axis);

            d = JMath.Clamp(d, 0, length);

            // get point on axis
            var R = p0 + axis * d;

            // distance
            var b = Math.Abs((centerA - R).Length());

            normal = (centerA - R) / b;

            // calculate closest 2 points
            var RH = JVector.Normalize(centerA - R);

            pointA = JVector.Negate(RH) * radiusA + centerA;
            pointB = RH * radiusB + R;

            normal.Negate();

            distance = b - (radiusA + radiusB);

            //
            if (b < radiusA + radiusB)
            {
                return(true);
            }
            return(false);
        }
コード例 #8
0
ファイル: XenoCollide.cs プロジェクト: Avatarchik/JPhysics
        /// <summary>
        /// Checks two shapes for collisions.
        /// </summary>
        /// <param name="support1">The SupportMappable implementation of the first shape to test.</param>
        /// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param>
        /// <param name="orientation1">The orientation of the first shape.</param>
        /// <param name="orientation2">The orientation of the second shape.</param>
        /// <param name="position1">The position of the first shape.</param>
        /// <param name="position2">The position of the second shape</param>
        /// <param name="point">The pointin world coordinates, where collision occur.</param>
        /// <param name="normal">The normal pointing from body2 to body1.</param>
        /// <param name="penetration">Estimated penetration depth of the collision.</param>
        /// <returns>Returns true if there is a collision, false otherwise.</returns>
        public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
                                  ref JMatrix orientation2, ref JVector position1, ref JVector position2,
                                  out JVector point, out JVector normal, out float penetration)
        {
            // Used variables
            JVector temp1, temp2;
            JVector v01, v02, v0;
            JVector v11, v12, v1;
            JVector v21, v22, v2;
            JVector v31, v32, v3;
            JVector v41, v42, v4;
            JVector mn;

            // Initialization of the output
            point       = normal = JVector.Zero;
            penetration = 0.0f;

            //JVector right = JVector.Right;

            // Get the center of shape1 in world coordinates -> v01
            support1.SupportCenter(out v01);
            JVector.Transform(ref v01, ref orientation1, out v01);
            JVector.Add(ref position1, ref v01, out v01);

            // Get the center of shape2 in world coordinates -> v02
            support2.SupportCenter(out v02);
            JVector.Transform(ref v02, ref orientation2, out v02);
            JVector.Add(ref position2, ref v02, out v02);

            // v0 is the center of the minkowski difference
            JVector.Subtract(ref v02, ref v01, out v0);

            // Avoid case where centers overlap -- any direction is fine in this case
            if (v0.IsNearlyZero())
            {
                v0 = new JVector(0.00001f, 0, 0);
            }

            // v1 = support in direction of origin
            mn = v0;
            JVector.Negate(ref v0, out normal);

            SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
            SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
            JVector.Subtract(ref v12, ref v11, out v1);

            if (JVector.Dot(ref v1, ref normal) <= 0.0f)
            {
                return(false);
            }

            // v2 = support perpendicular to v1,v0
            JVector.Cross(ref v1, ref v0, out normal);

            if (normal.IsNearlyZero())
            {
                JVector.Subtract(ref v1, ref v0, out normal);

                normal.Normalize();

                point = v11;
                JVector.Add(ref point, ref v12, out point);
                JVector.Multiply(ref point, 0.5f, out point);

                JVector.Subtract(ref v12, ref v11, out temp1);
                penetration = JVector.Dot(ref temp1, ref normal);

                //point = v11;
                //point2 = v12;
                return(true);
            }

            JVector.Negate(ref normal, out mn);
            SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21);
            SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22);
            JVector.Subtract(ref v22, ref v21, out v2);

            if (JVector.Dot(ref v2, ref normal) <= 0.0f)
            {
                return(false);
            }

            // Determine whether origin is on + or - side of plane (v1,v0,v2)
            JVector.Subtract(ref v1, ref v0, out temp1);
            JVector.Subtract(ref v2, ref v0, out temp2);
            JVector.Cross(ref temp1, ref temp2, out normal);

            float dist = JVector.Dot(ref normal, ref v0);

            // If the origin is on the - side of the plane, reverse the direction of the plane
            if (dist > 0.0f)
            {
                JVector.Swap(ref v1, ref v2);
                JVector.Swap(ref v11, ref v21);
                JVector.Swap(ref v12, ref v22);
                JVector.Negate(ref normal, out normal);
            }


            int  phase2 = 0;
            int  phase1 = 0;
            bool hit    = false;

            // Phase One: Identify a portal
            while (true)
            {
                if (phase1 > MaximumIterations)
                {
                    return(false);
                }

                phase1++;

                // Obtain the support point in a direction perpendicular to the existing plane
                // Note: This point is guaranteed to lie off the plane
                JVector.Negate(ref normal, out mn);
                SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
                SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
                JVector.Subtract(ref v32, ref v31, out v3);

                if (JVector.Dot(ref v3, ref normal) <= 0.0f)
                {
                    return(false);
                }

                // If origin is outside (v1,v0,v3), then eliminate v2 and loop
                JVector.Cross(ref v1, ref v3, out temp1);
                if (JVector.Dot(ref temp1, ref v0) < 0.0f)
                {
                    v2  = v3;
                    v21 = v31;
                    v22 = v32;
                    JVector.Subtract(ref v1, ref v0, out temp1);
                    JVector.Subtract(ref v3, ref v0, out temp2);
                    JVector.Cross(ref temp1, ref temp2, out normal);
                    continue;
                }

                // If origin is outside (v3,v0,v2), then eliminate v1 and loop
                JVector.Cross(ref v3, ref v2, out temp1);
                if (JVector.Dot(ref temp1, ref v0) < 0.0f)
                {
                    v1  = v3;
                    v11 = v31;
                    v12 = v32;
                    JVector.Subtract(ref v3, ref v0, out temp1);
                    JVector.Subtract(ref v2, ref v0, out temp2);
                    JVector.Cross(ref temp1, ref temp2, out normal);
                    continue;
                }

                // Phase Two: Refine the portal
                // We are now inside of a wedge...
                while (true)
                {
                    phase2++;

                    // Compute normal of the wedge face
                    JVector.Subtract(ref v2, ref v1, out temp1);
                    JVector.Subtract(ref v3, ref v1, out temp2);
                    JVector.Cross(ref temp1, ref temp2, out normal);

                    // Can this happen???  Can it be handled more cleanly?
                    if (normal.IsNearlyZero())
                    {
                        return(true);
                    }

                    normal.Normalize();

                    // Compute distance from origin to wedge face
                    float d = JVector.Dot(ref normal, ref v1);


                    // If the origin is inside the wedge, we have a hit
                    if (d >= 0 && !hit)
                    {
                        // HIT!!!
                        hit = true;
                    }

                    // Find the support point in the direction of the wedge face
                    JVector.Negate(ref normal, out mn);
                    SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v41);
                    SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v42);
                    JVector.Subtract(ref v42, ref v41, out v4);

                    JVector.Subtract(ref v4, ref v3, out temp1);
                    float delta = JVector.Dot(ref temp1, ref normal);
                    penetration = JVector.Dot(ref v4, ref normal);

                    // If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
                    if (delta <= CollideEpsilon || penetration <= 0.0f || phase2 > MaximumIterations)
                    {
                        if (hit)
                        {
                            JVector.Cross(ref v1, ref v2, out temp1);
                            float b0 = JVector.Dot(ref temp1, ref v3);
                            JVector.Cross(ref v3, ref v2, out temp1);
                            float b1 = JVector.Dot(ref temp1, ref v0);
                            JVector.Cross(ref v0, ref v1, out temp1);
                            float b2 = JVector.Dot(ref temp1, ref v3);
                            JVector.Cross(ref v2, ref v1, out temp1);
                            float b3 = JVector.Dot(ref temp1, ref v0);

                            float sum = b0 + b1 + b2 + b3;

                            if (sum <= 0)
                            {
                                b0 = 0;
                                JVector.Cross(ref v2, ref v3, out temp1);
                                b1 = JVector.Dot(ref temp1, ref normal);
                                JVector.Cross(ref v3, ref v1, out temp1);
                                b2 = JVector.Dot(ref temp1, ref normal);
                                JVector.Cross(ref v1, ref v2, out temp1);
                                b3 = JVector.Dot(ref temp1, ref normal);

                                sum = b1 + b2 + b3;
                            }

                            float inv = 1.0f / sum;

                            JVector.Multiply(ref v01, b0, out point);
                            JVector.Multiply(ref v11, b1, out temp1);
                            JVector.Add(ref point, ref temp1, out point);
                            JVector.Multiply(ref v21, b2, out temp1);
                            JVector.Add(ref point, ref temp1, out point);
                            JVector.Multiply(ref v31, b3, out temp1);
                            JVector.Add(ref point, ref temp1, out point);

                            JVector.Multiply(ref v02, b0, out temp2);
                            JVector.Add(ref temp2, ref point, out point);
                            JVector.Multiply(ref v12, b1, out temp1);
                            JVector.Add(ref point, ref temp1, out point);
                            JVector.Multiply(ref v22, b2, out temp1);
                            JVector.Add(ref point, ref temp1, out point);
                            JVector.Multiply(ref v32, b3, out temp1);
                            JVector.Add(ref point, ref temp1, out point);

                            JVector.Multiply(ref point, inv * 0.5f, out point);
                        }

                        // Compute the barycentric coordinates of the origin
                        return(hit);
                    }

                    ////// Compute the tetrahedron dividing face (v4,v0,v1)
                    //JVector.Cross(ref v4, ref v1, out temp1);
                    //float d1 = JVector.Dot(ref temp1, ref v0);


                    ////// Compute the tetrahedron dividing face (v4,v0,v2)
                    //JVector.Cross(ref v4, ref v2, out temp1);
                    //float d2 = JVector.Dot(ref temp1, ref v0);


                    // Compute the tetrahedron dividing face (v4,v0,v3)
                    JVector.Cross(ref v4, ref v0, out temp1);
                    float dot = JVector.Dot(ref temp1, ref v1);

                    if (dot >= 0.0f)
                    {
                        dot = JVector.Dot(ref temp1, ref v2);

                        if (dot >= 0.0f)
                        {
                            // Inside d1 & inside d2 ==> eliminate v1
                            v1  = v4;
                            v11 = v41;
                            v12 = v42;
                        }
                        else
                        {
                            // Inside d1 & outside d2 ==> eliminate v3
                            v3  = v4;
                            v31 = v41;
                            v32 = v42;
                        }
                    }
                    else
                    {
                        dot = JVector.Dot(ref temp1, ref v3);

                        if (dot >= 0.0f)
                        {
                            // Outside d1 & inside d3 ==> eliminate v2
                            v2  = v4;
                            v21 = v41;
                            v22 = v42;
                        }
                        else
                        {
                            // Outside d1 & outside d3 ==> eliminate v1
                            v1  = v4;
                            v11 = v41;
                            v12 = v42;
                        }
                    }
                }
            }
        }
コード例 #9
0
        public override bool Raycast(RigidBody body, JVector rayOrigin, JVector rayDirection, out JVector normal, out float fraction)
        {
            fraction = float.MaxValue; normal = JVector.Zero;

            if (!body.BoundingBox.RayIntersect(rayOrigin, rayDirection))
            {
                return(false);
            }

            if (body.Shape is Multishape multishape)
            {
                multishape = multishape.RequestWorkingClone();

                bool multiShapeCollides = false;

                JVector.Subtract(rayOrigin, body.position, out var transformedOrigin);
                JVector.Transform(transformedOrigin, body.invOrientation, out transformedOrigin);
                JVector.Transform(rayDirection, body.invOrientation, out var transformedDirection);

                int msLength = multishape.Prepare(transformedOrigin, transformedDirection);

                for (int i = 0; i < msLength; i++)
                {
                    multishape.SetCurrentShape(i);

                    if (GJKCollide.Raycast(
                            multishape,
                            body.orientation,
                            body.position,
                            rayOrigin,
                            rayDirection,
                            out float tempFraction,
                            out var tempNormal) &&
                        tempFraction < fraction)
                    {
                        if (useTerrainNormal && multishape is TerrainShape terrainShape)
                        {
                            terrainShape.CollisionNormal(out tempNormal);
                            JVector.Transform(tempNormal, body.orientation, out tempNormal);
                            tempNormal = JVector.Negate(tempNormal);
                        }
                        else if (useTriangleMeshNormal && multishape is TriangleMeshShape triangleMeshShape)
                        {
                            triangleMeshShape.CollisionNormal(out tempNormal);
                            JVector.Transform(tempNormal, body.orientation, out tempNormal);
                            tempNormal = JVector.Negate(tempNormal);
                        }

                        normal             = tempNormal;
                        fraction           = tempFraction;
                        multiShapeCollides = true;
                    }
                }

                multishape.ReturnWorkingClone();
                return(multiShapeCollides);
            }
            else
            {
                return(GJKCollide.Raycast(
                           body.Shape,
                           body.orientation,
                           body.position,
                           rayOrigin,
                           rayDirection,
                           out fraction,
                           out normal));
            }
        }
コード例 #10
0
        /*
         * private ResourcePool<List<int>> potentialTriangleLists = new ResourcePool<List<int>>();
         *
         * private void DetectSoftSoft(SoftBody body1, SoftBody body2)
         * {
         *  List<int> my = potentialTriangleLists.GetNew();
         *  List<int> other = potentialTriangleLists.GetNew();
         *
         *  body1.dynamicTree.Query(other, my, body2.dynamicTree);
         *
         *  for (int i = 0; i < other.Count; i++)
         *  {
         *      SoftBody.Triangle myTriangle = body1.dynamicTree.GetUserData(my[i]);
         *      SoftBody.Triangle otherTriangle = body2.dynamicTree.GetUserData(other[i]);
         *
         *      JVector point, normal;
         *      float penetration;
         *      bool result;
         *
         *      result = XenoCollide.Detect(myTriangle, otherTriangle, ref JMatrix.InternalIdentity, ref JMatrix.InternalIdentity,
         *          ref JVector.InternalZero, ref JVector.InternalZero, out point, out normal, out penetration);
         *
         *      if (result)
         *      {
         *          int minIndexMy = FindNearestTrianglePoint(body1, my[i], ref point);
         *          int minIndexOther = FindNearestTrianglePoint(body2, other[i], ref point);
         *
         *          if (this.RaisePassedNarrowphase(body1.points[minIndexMy], body2.points[minIndexOther],
         *              ref point, ref normal, penetration))
         *          {
         *              RaiseCollisionDetected(body1.points[minIndexMy],
         *                  body2.points[minIndexOther], ref point, ref point, ref normal, penetration);
         *          }
         *      }
         *  }
         *
         *  my.Clear(); other.Clear();
         *  potentialTriangleLists.GiveBack(my);
         *  potentialTriangleLists.GiveBack(other);
         * }
         */


        private void DetectRigidRigid(RigidBody body1, RigidBody body2)
        {
            // we don't support multi shapes yet!
            bool b1IsMulti = false; // (body1.Shape is Multishape);
            bool b2IsMulti = false; // (body2.Shape is Multishape);

            bool speculative = speculativeContacts ||
                               (body1.EnableSpeculativeContacts || body2.EnableSpeculativeContacts);

            JVector point       = JVector.Zero;
            JVector normal      = JVector.Zero;
            float   penetration = 0;

            if (!b1IsMulti && !b2IsMulti)
            {
                JVector point1 = JVector.Zero;
                JVector point2 = JVector.Zero;

                JMatrix OA   = JMatrix.CreateRotationZ(body1.orientation);
                JMatrix OB   = JMatrix.CreateRotationZ(body2.orientation);
                JVector zero = JVector.Zero;
                float   t    = 0.0f;

                if (body1.Shape.type == ShapeType.Box && body2.Shape.type == ShapeType.Box)
                {
                    var A = body1.Shape as BoxShape;
                    var B = body2.Shape as BoxShape;

                    A.UpdateAxes(body1.orientation);
                    B.UpdateAxes(body2.orientation);

                    JVector[] CA   = new JVector[2], CB = new JVector[2];
                    int       Cnum = 0;

                    if (Collision.BoxBoxTestContact(ref A, ref body1.position, ref OA,
                                                    ref B, ref body2.position, ref OB,
                                                    out normal, out t, out CA, out CB, out Cnum))
                    {
                        normal.Negate();
                        RaiseCollisionDetected(body1, body2, ref CA[0], ref CB[0], ref normal, -t);
                    }
                }
                else if (body1.Shape.type == ShapeType.Circle && body2.Shape.type == ShapeType.Circle)
                {
                    var A = body1.Shape as CircleShape;
                    var B = body2.Shape as CircleShape;

                    Collision.CircleCircleTest(body1.position, A.Radius, body2.position, B.Radius, out point1, out point2, out normal, out penetration);

                    if (penetration < 0)
                    {
                        RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, -penetration);
                    }
                }
                // all shapes (GJK)
                else if (speculative)
                {
                    //JVector hit1, hit2;

                    //if (GJKCollide.ClosestPoints(body1.Shape, body2.Shape, ref OA, ref OB,
                    //    ref body1.position, ref body2.position, out hit1, out hit2, out normal))
                    //{
                    //    JVector delta = hit2 - hit1;

                    //    if (delta.LengthSquared() < (body1.sweptDirection - body2.sweptDirection).LengthSquared())
                    //    {
                    //        //normal.Negate();
                    //        penetration = delta * normal;

                    //        if (penetration < 0.0f)
                    //        {
                    //            RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration);
                    //        }
                    //    }
                    //}
                }
            }

            else if (b1IsMulti && b2IsMulti)
            {
                /*
                 * Multishape ms1 = (body1.Shape as Multishape);
                 * Multishape ms2 = (body2.Shape as Multishape);
                 *
                 * ms1 = ms1.RequestWorkingClone();
                 * ms2 = ms2.RequestWorkingClone();
                 *
                 * JBBox transformedBoundingBox = body2.boundingBox;
                 * transformedBoundingBox.InverseTransform(ref body1.position, ref body1.orientation);
                 *
                 * int ms1Length = ms1.Prepare(ref transformedBoundingBox);
                 *
                 * transformedBoundingBox = body1.boundingBox;
                 * transformedBoundingBox.InverseTransform(ref body2.position, ref body2.orientation);
                 *
                 * int ms2Length = ms2.Prepare(ref transformedBoundingBox);
                 *
                 * if (ms1Length == 0 || ms2Length == 0)
                 * {
                 *  ms1.ReturnWorkingClone();
                 *  ms2.ReturnWorkingClone();
                 *  return;
                 * }
                 *
                 * for (int i = 0; i < ms1Length; i++)
                 * {
                 *  ms1.SetCurrentShape(i);
                 *
                 *  for (int e = 0; e < ms2Length; e++)
                 *  {
                 *      ms2.SetCurrentShape(e);
                 *
                 *      if (XenoCollide.Detect(ms1, ms2, ref body1.orientation,
                 *          ref body2.orientation, ref body1.position, ref body2.position,
                 *          out point, out normal, out penetration))
                 *      {
                 *          if (this.RaisePassedNarrowphase(body1, body2, ref point, ref normal, penetration))
                 *          {
                 *              JVector point1, point2;
                 *              FindSupportPoints(body1, body2, ms1, ms2, ref point, ref normal, out point1, out point2);
                 *
                 *              RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration);
                 *          }
                 *      }
                 *  }
                 * }
                 *
                 * ms1.ReturnWorkingClone();
                 * ms2.ReturnWorkingClone();
                 * */
            }
            else
            {
                /*
                 * RigidBody b1, b2;
                 *
                 * if (body2.Shape is Multishape) { b1 = body2; b2 = body1; }
                 * else { b2 = body2; b1 = body1; }
                 *
                 * Multishape ms = (b1.Shape as Multishape);
                 *
                 * ms = ms.RequestWorkingClone();
                 *
                 * JBBox transformedBoundingBox = b2.boundingBox;
                 * transformedBoundingBox.InverseTransform(ref b1.position, ref b1.orientation);
                 *
                 * int msLength = ms.Prepare(ref transformedBoundingBox);
                 *
                 * if (msLength == 0)
                 * {
                 *  ms.ReturnWorkingClone();
                 *  return;
                 * }
                 *
                 * for (int i = 0; i < msLength; i++)
                 * {
                 *  ms.SetCurrentShape(i);
                 *
                 *  if (XenoCollide.Detect(ms, b2.Shape, ref b1.orientation,
                 *      ref b2.orientation, ref b1.position, ref b2.position,
                 *      out point, out normal, out penetration))
                 *  {
                 *      if (this.RaisePassedNarrowphase(b1, b2, ref point, ref normal, penetration))
                 *      {
                 *          JVector point1, point2;
                 *          FindSupportPoints(b1, b2, ms, b2.Shape, ref point, ref normal, out point1, out point2);
                 *
                 *          if (useTerrainNormal && ms is TerrainShape)
                 *          {
                 *              (ms as TerrainShape).CollisionNormal(out normal);
                 *              JVector.Transform(ref normal, ref b1.orientation, out normal);
                 *          }
                 *          else if (useTriangleMeshNormal && ms is TriangleMeshShape)
                 *          {
                 *              (ms as TriangleMeshShape).CollisionNormal(out normal);
                 *              JVector.Transform(ref normal, ref b1.orientation, out normal);
                 *          }
                 *
                 *          RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration);
                 *      }
                 *  }
                 * }
                 *
                 * ms.ReturnWorkingClone();
                 * */
            }
        }
コード例 #11
0
ファイル: GJKCollide.cs プロジェクト: LaserYGD/Zeldo-CSharp
        public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
                                         ref JMatrix orientation2, ref JVector position1, ref JVector position2,
                                         out JVector p1, out JVector p2, out JVector normal)
        {
            VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();

            simplexSolver.Reset();

            p1 = p2 = JVector.Zero;

            JVector r = position1 - position2;
            JVector w, v;

            JVector supVertexA;
            JVector rn, vn;

            rn = JVector.Negate(r);

            SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA);

            JVector supVertexB;

            SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB);

            v = supVertexA - supVertexB;

            normal = JVector.Zero;

            int maxIter = 15;

            float distSq  = v.LengthSquared();
            float epsilon = 0.00001f;

            while ((distSq > epsilon) && (maxIter-- != 0))
            {
                vn = JVector.Negate(v);
                SupportMapTransformed(support1, ref orientation1, ref position1, ref vn, out supVertexA);
                SupportMapTransformed(support2, ref orientation2, ref position2, ref v, out supVertexB);
                w = supVertexA - supVertexB;

                if (!simplexSolver.InSimplex(w))
                {
                    simplexSolver.AddVertex(w, supVertexA, supVertexB);
                }
                if (simplexSolver.Closest(out v))
                {
                    distSq = v.LengthSquared();
                    normal = v;
                }
                else
                {
                    distSq = 0.0f;
                }
            }


            simplexSolver.ComputePoints(out p1, out p2);

            if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon)
            {
                normal.Normalize();
            }

            simplexSolverPool.GiveBack(simplexSolver);

            return(true);
        }
コード例 #12
0
        /// <summary>
        /// Checks two shapes for collisions.
        /// </summary>
        /// <param name="support1">The SupportMappable implementation of the first shape to test.</param>
        /// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param>
        /// <param name="orientation1">The orientation of the first shape.</param>
        /// <param name="orientation2">The orientation of the second shape.</param>
        /// <param name="position1">The position of the first shape.</param>
        /// <param name="position2">The position of the second shape</param>
        /// <param name="point">The pointin world coordinates, where collision occur.</param>
        /// <param name="normal">The normal pointing from body2 to body1.</param>
        /// <param name="penetration">Estimated penetration depth of the collision.</param>
        /// <returns>Returns true if there is a collision, false otherwise.</returns>
        public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
                                  ref JMatrix orientation2, ref JVector position1, ref JVector position2,
                                  out JVector point, out JVector normal, out float penetration)
        {
            // Used variables
            JVector v01, v02, v0;
            JVector v11, v12, v1;
            JVector v21, v22, v2;
            JVector v31, v32, v3;
            JVector mn;

            // Initialization of the output
            point       = normal = JVector.Zero;
            penetration = 0.0f;

            // Get the center of shape1 in world coordinates -> v01
            support1.SupportCenter(out v01);
            JVector.Transform(ref v01, ref orientation1, out v01);
            JVector.Add(ref position1, ref v01, out v01);

            // Get the center of shape2 in world coordinates -> v02
            support2.SupportCenter(out v02);
            JVector.Transform(ref v02, ref orientation2, out v02);
            JVector.Add(ref position2, ref v02, out v02);

            // v0 is the center of the minkowski difference
            JVector.Subtract(ref v02, ref v01, out v0);

            // Avoid case where centers overlap -- any direction is fine in this case
            if (v0.IsNearlyZero())
            {
                v0 = new JVector(0.00001f, 0);
            }

            // v1 = support in direction of origin
            mn = v0;
            JVector.Negate(ref v0, out normal);

            SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
            SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
            JVector.Subtract(ref v12, ref v11, out v1);

            if (JVector.Dot(ref v1, ref normal) <= 0.0f)
            {
                return(false);
            }

            // v2 = support perpendicular to v1,v0
            normal = OutsidePortal(v1, v0);

            JVector.Negate(ref normal, out mn);
            SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21);
            SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22);
            JVector.Subtract(ref v22, ref v21, out v2);

            //LD.Draw(Conversion.ToXNAVector2(v1), Conversion.ToXNAVector2(v0), Color.Blue);
            //LD.Draw(Conversion.ToXNAVector2(v2), Conversion.ToXNAVector2(v0), Color.Blue);

            if (JVector.Dot(ref v2, ref normal) <= 0.0f)
            {
                return(false);
            }

            // phase two: portal refinement
            int maxIterations = 0;

            while (true)
            {
                // find normal direction
                if (!IntersectPortal(v0, v2, v1))
                {
                    normal = InsidePortal(v2, v1);
                }
                else
                {
                    // origin ray crosses the portal
                    normal = OutsidePortal(v2, v1);
                }


                // obtain the next support point
                JVector.Negate(ref normal, out mn);
                SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
                SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
                JVector.Subtract(ref v32, ref v31, out v3);

                //LD.Draw(Conversion.ToXNAVector2(v3), Conversion.ToXNAVector2(v0), Color.Green);

                if (JVector.Dot(v3, normal) <= 0)
                {
                    JVector ab = v3 - v2;
                    float   t  = -(JVector.Dot(v2, ab)) / (JVector.Dot(ab, ab));
                    normal = (v2 + (t * ab));
                    return(false);
                }

                // Portal lies on the outside edge of the Minkowski Hull.
                // Return contact information
                if (JVector.Dot((v3 - v2), normal) <= CollideEpsilon || ++maxIterations > MaximumIterations)
                {
                    JVector ab = v2 - v1;
                    float   t  = JVector.Dot(JVector.Negate(v1), ab);
                    if (t <= 0.0f)
                    {
                        t      = 0.0f;
                        normal = v1;
                    }
                    else
                    {
                        float denom = JVector.Dot(ab, ab);
                        if (t >= denom)
                        {
                            normal = v2;
                            t      = 1.0f;
                        }
                        else
                        {
                            t     /= denom;
                            normal = v1 + t * ab;
                        }
                    }

                    float s = 1 - t;

                    point = s * v11 + t * v21;
                    var point2 = s * v12 + t * v22;

                    // this  causes a sq root = bad!
                    penetration = normal.Length();
                    normal.Normalize();
                    return(true);
                }

                // if origin is inside (v1, v0, v3), refine portal
                if (OriginInTriangle(v0, v1, v3))
                {
                    v2  = v3;
                    v21 = v31;
                    v22 = v32;
                    continue;
                }
                // if origin is inside (v3, v0, v2), refine portal
                else if (OriginInTriangle(v0, v2, v3))
                {
                    v1  = v3;
                    v11 = v31;
                    v12 = v32;
                    continue;
                }
                return(false);
            }
        }