예제 #1
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 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;
                        }
                    }
                }
            }
        }
예제 #2
0
        /// <summary>
        /// Adds a body to the fluid. Only bodies which where added
        /// to the fluidvolume gets affected by buoyancy forces.
        /// </summary>
        /// <param name="body">The body which should be added.</param>
        /// <param name="subdivisions">The object is subdivided in smaller objects
        /// for which buoyancy force is calculated. The more subdivisons the better
        /// the results. Note that the total number of subdivisions is subdivisions³.</param>
        public void Add(RigidBody body, int subdivisions)
        {
            List <JVector> massPoints = new List <JVector>();
            JVector        testVector;

            JVector diff = body.Shape.BoundingBox.Max - body.Shape.BoundingBox.Min;

            if (diff.IsNearlyZero())
            {
                throw new InvalidOperationException("BoundingBox volume of the shape is zero.");
            }

            Multishape ms     = body.Shape as Multishape;
            int        values = 0;

            if (ms != null)
            {
                JBBox largeBox = JBBox.LargeBox;
                values = ms.Prepare(ref largeBox);
            }

            for (int i = 0; i < subdivisions; i++)
            {
                for (int e = 0; e < subdivisions; e++)
                {
                    for (int k = 0; k < subdivisions; k++)
                    {
                        testVector.X = body.Shape.BoundingBox.Min.X + (diff.X / (float)(subdivisions - 1)) * ((float)i);
                        testVector.Y = body.Shape.BoundingBox.Min.Y + (diff.Y / (float)(subdivisions - 1)) * ((float)e);
                        testVector.Z = body.Shape.BoundingBox.Min.Z + (diff.Z / (float)(subdivisions - 1)) * ((float)k);

                        JMatrix ident = JMatrix.Identity;
                        JVector zero  = JVector.Zero;

                        if (ms != null)
                        {
                            for (int j = 0; j < values; j++)
                            {
                                ms.SetCurrentShape(j);


                                if (GJKCollide.Pointcast(body.Shape, ref ident,
                                                         ref zero, ref testVector))
                                {
                                    massPoints.Add(testVector);
                                }
                            }
                        }
                        else
                        {
                            if (GJKCollide.Pointcast(body.Shape, ref ident,
                                                     ref zero, ref testVector))
                            {
                                massPoints.Add(testVector);
                            }
                        }
                    }
                }
            }

            samples.Add(body.Shape, massPoints.ToArray());
            bodies.Add(body);
        }