Exemple #1
0
 public Triangle(Vector3 pos1, Vector3 pos2, Vector3 pos3)
 {
     Pos1   = pos1;
     Pos2   = pos2;
     Pos3   = pos3;
     Normal = VMath.Cross(Pos3 - Pos1, Pos2 - Pos1);
 }
Exemple #2
0
 public void ApplyImpulse(Vector2R impulse, Vector2R contactVector)
 {
     //
     velocity += invmass * impulse;
     //if (float.IsNaN(velocity.X)) System.Diagnostics.Debugger.Break();
     angularVelocity += invinertia * (float)VMath.Cross(contactVector, impulse);
     //if (float.IsInfinity(angularVelocity)) System.Diagnostics.Debugger.Break();
 }
Exemple #3
0
        public bool IsIntersectWithRay(Ray ray)
        {
            var cross1 = VMath.Cross(Pos2 - Pos1, ray.pos - Pos1);
            var dot1   = VMath.Dot(cross1, ray.vec);

            var cross2 = VMath.Cross(Pos3 - Pos2, ray.pos - Pos2);
            var dot2   = VMath.Dot(cross2, ray.vec);

            if (dot1 > 0 ^ dot2 > 0)
            {
                return(false);
            }

            var cross3 = VMath.Cross(Pos1 - Pos3, ray.pos - Pos3);
            var dot3   = VMath.Dot(cross3, ray.vec);

            return(!(dot2 > 0 ^ dot3 > 0));
        }
Exemple #4
0
        public override void ComputeMass(float density)
        {
            //calculate centroid and moment of inertia
            Vector2R c      = new Vector2R(0, 0); // centroid
            double   area   = 0;
            double   I      = 0;
            double   k_inv3 = 1.0 / 3.0;

            for (int i1 = 0; i1 < vertexCount; i1++)
            {
                Vector2R p1 = vertices[i1];
                int      i2 = i1 + 1 < vertexCount ? i1 + 1 : 0;
                Vector2R p2 = vertices[i2];

                double D            = VMath.Cross(p1, p2);
                double triangleArea = 0.5 * D;

                area += triangleArea;

                //use area to weight the centroid average, not just the vertex position
                c += VMath.MultVectDouble(p1 + p2, triangleArea * k_inv3); // triangleArea * k_inv3 * (p1 + p2);

                double intx2 = p1.X * p1.X + p2.X * p1.X + p2.X * p2.X;
                double inty2 = p1.Y * p1.Y + p2.Y * p1.Y + p2.Y * p2.Y;
                I += (0.25 * k_inv3 * D) * (intx2 + inty2);
            }
            c = VMath.MultVectDouble(c, 1.0 / area);

            //translate verticies to centroid (make centroid (0,0)
            //for the polygon in model space)
            //Not really necessary but I like doing this anyway
            for (int i = 0; i < vertexCount; i++)
            {
                vertices[i] -= c;
            }

            body.mass    = density * (float)area;
            body.inertia = (float)I * density;
        }
Exemple #5
0
        public void Initialize()
        {
            e  = Math.Min(a.restitution, b.restitution);
            sf = Math.Sqrt(a.staticFriction * a.staticFriction);
            df = Math.Sqrt(a.dynamicFriction * a.dynamicFriction);

            for (int i = 0; i < contact_count; i++)
            {
                Vector2R ra = contacts[i] - a.pos;
                Vector2R rb = contacts[i] - b.pos;

                Vector2R crossprod_b = VMath.Cross(b.angularVelocity, rb);
                Vector2R crossprod_a = VMath.Cross(a.angularVelocity, ra);
                Vector2R rv          = b.velocity + crossprod_b - a.velocity - crossprod_a;

                //Vector2 rv = b.velocity + VMath.Cross(b.angularVelocity, rb) -
                //             a.velocity - VMath.Cross(a.angularVelocity, ra);
                //if (b.velocity.IsFucked()) System.Diagnostics.Debugger.Break(); //removelater
                //if (a.velocity.IsFucked()) System.Diagnostics.Debugger.Break(); //removelater

                //if (rv.Length() < (dt,gravity).LengthSquared() + EPSILON) e = 0.0f;
            }
        }
Exemple #6
0
        public void ApplyImpulse()
        {
            if (GMath.Equal(a.invmass + b.invmass, 0))
            {
                InfinitMassCorrection();
                return;
            }

            for (int i = 0; i < contact_count; i++)
            {
                //calcuate radii from COM to contact
                Vector2R ra = contacts[i] - a.pos;
                Vector2R rb = contacts[i] - b.pos;
                //relative velocity
                Vector2R rv = b.velocity + VMath.Cross(b.angularVelocity, rb) -
                              a.velocity - VMath.Cross(a.angularVelocity, ra);
                //relative velocity along the normal
                double contactVel = Vector2R.Dot(rv, normal);
                //do not resolve if velocities are seperating

                if (contactVel > 0)
                {
                    return;
                }

                double raCrossN   = VMath.Cross(ra, normal);
                double rbCrossN   = VMath.Cross(rb, normal);
                double invMassSum = a.invmass + b.invmass + (raCrossN * raCrossN) * a.invinertia + (rbCrossN * rbCrossN) * b.invinertia;
                //calculate impulse scalar
                double j = -(1.0 + e) * contactVel;
                j /= invMassSum;
                j /= (double)contact_count;
                //apply impulse
                Vector2R impulse = VMath.MultVectDouble(normal, j); // normal * j;
                a.ApplyImpulse(-impulse, ra);
                b.ApplyImpulse(impulse, rb);
                //friction impulse
                rv = b.velocity + VMath.Cross(b.angularVelocity, rb) -
                     a.velocity - VMath.Cross(a.angularVelocity, ra);
                Vector2R t = rv - (normal * Vector2R.Dot(rv, normal));
                //t.Normalize();
                VMath.NormalizeSafe(ref t);
                //j tangent magnitude
                double jt = -Vector2R.Dot(rv, t);
                jt /= invMassSum;
                jt /= (double)contact_count;
                //don't apply tiny friction impulses
                if (GMath.Equal(jt, 0.0))
                {
                    return;
                }
                //coulumbs law
                Vector2R tangentImpulse;
                if (Math.Abs(jt) < j * sf)
                {
                    tangentImpulse = VMath.MultVectDouble(t, df); // t * df;
                }
                else
                {
                    tangentImpulse = VMath.MultVectDouble(t, -j * df); // t * -j * df
                }
                //apply friction impulse
                a.ApplyImpulse(-tangentImpulse, ra);
                b.ApplyImpulse(tangentImpulse, rb);
            }
        }
Exemple #7
0
        public void Set(Vector2R[] verts, int count)
        {
            //no hulls with less than 3 verticies (ensure actual polygon)
            //Debug.Assert(count > 2 && count < MaxPolyVertexCount);
            count = Math.Min(count, MaxPolyVertexCount);

            //find the right most point in the hull
            int    rightMost     = 0;
            double highestXCoord = verts[0].X;

            for (int i = 1; i < count; i++)
            {
                double x = verts[0].X;
                if (x > highestXCoord)
                {
                    highestXCoord = x;
                    rightMost     = i;
                }
                //if matching x then take farthest negative y
                else if (x == highestXCoord && verts[i].Y < verts[rightMost].Y)
                {
                    rightMost = i;
                }
            }

            int[] hull      = new int[MaxPolyVertexCount];
            int   outCount  = 0;
            int   indexHull = rightMost;

            for (;;)
            {
                hull[outCount] = indexHull;
                // search for next index that wraps around the hull
                // by computing cross products to find the most counter-clockwise
                // vertex in the set, given the previous hull index
                int nextHullIndex = 0;
                for (int i = 1; i < count; i++)
                {
                    //skip if same coordinate as we need three unique
                    //points in the set to perform a cross product
                    if (nextHullIndex == indexHull)
                    {
                        nextHullIndex = i;
                        continue;
                    }
                    // cross every set of three unquie verticies
                    // record each counter clockwise third vertex and add
                    // to the output hull
                    Vector2R e1 = verts[nextHullIndex] - verts[hull[outCount]];
                    Vector2R e2 = verts[i] - verts[hull[outCount]];
                    double   c  = VMath.Cross(e1, e2);
                    if (c < 0.0f)
                    {
                        nextHullIndex = i;
                    }

                    // Cross product is zero then e vectors are on same line
                    // therefor want to record vertex farthest along that line
                    if (c == 0.0f && e2.LengthSquared() > e1.LengthSquared())
                    {
                        nextHullIndex = i;
                    }
                }
                outCount++;
                indexHull = nextHullIndex;
                //conclude algorithm upon wraparound
                if (nextHullIndex == rightMost)
                {
                    vertexCount = outCount;
                    break;
                }
            }
            float maxDist = 0;

            // Copy vertices into shape's vertices
            for (int i = 0; i < vertexCount; ++i)
            {
                vertices[i] = verts[hull[i]];
                float dist = Vector2R.Distance(Vector2R.Zero, vertices[i]);
                if (dist > maxDist)
                {
                    maxDist = dist;
                }
            }
            polyReach = maxDist * 2;

            ComputeNormals();
        }