예제 #1
0
        public override void ApplyInternalForces(double elapsed)
        {
            base.ApplyInternalForces(elapsed);

            // internal forces based on pressure equations.  we need 2 loops to do this.  one to find the overall volume of the
            // body, and 1 to apply forces.  we will need the normals for the edges in both loops, so we will cache them and remember them.
            volume = 0f;

            for (int i = 0; i < count; i++)
            {
                int prev = (i > 0) ? i - 1 : count - 1;
                int next = (i < count - 1) ? i + 1 : 0;

                // currently we are talking about the edge from i --> j.
                // first calculate the volume of the body, and cache normals as we go.
                Vector2 edge1N = new Vector2();
                edge1N.X = pointmass_list[i].position.X - pointmass_list[prev].position.X;
                edge1N.Y = pointmass_list[i].position.Y - pointmass_list[prev].position.Y;
                VectorHelper.Perpendicular(ref edge1N);

                Vector2 edge2N = new Vector2();
                edge2N.X = pointmass_list[next].position.X - pointmass_list[i].position.X;
                edge2N.Y = pointmass_list[next].position.Y - pointmass_list[i].position.Y;
                VectorHelper.Perpendicular(ref edge2N);

                Vector2 norm = new Vector2();
                norm.X = edge1N.X + edge2N.X;
                norm.Y = edge1N.Y + edge2N.Y;

                float nL = (float)Math.Sqrt((norm.X * norm.X) + (norm.Y * norm.Y));
                if (nL > 0.001f)
                {
                    norm.X /= nL;
                    norm.Y /= nL;
                }

                float edgeL = (float)Math.Sqrt((edge2N.X * edge2N.X) + (edge2N.Y * edge2N.Y));

                // cache normal and edge length
                normal_list[i]     = norm;
                edgelength_list[i] = edgeL;

                float xdist = Math.Abs(pointmass_list[i].position.X - pointmass_list[next].position.X);

                float volumeProduct = xdist * Math.Abs(norm.X) * edgeL;

                // add to volume
                volume += 0.5f * volumeProduct;
            }

            // now loop through, adding forces!
            float invVolume = 1f / volume;

            for (int i = 0; i < count; i++)
            {
                int j = (i < count - 1) ? i + 1 : 0;

                float pressureV = (invVolume * edgelength_list[i] * pressure);
                pointmass_list[i].force.X += normal_list[i].X * pressureV;
                pointmass_list[i].force.Y += normal_list[i].Y * pressureV;

                pointmass_list[j].force.X += normal_list[j].X * pressureV;
                pointmass_list[j].force.Y += normal_list[j].Y * pressureV;
            }
        }
예제 #2
0
        public static List <CollisionInfo> Intersects(Body body_a, Body body_b)
        {
            List <CollisionInfo> data = new List <CollisionInfo>();

            int bApmCount = body_a.count;
            int bBpmCount = body_b.count;

            BoundingSquare boxB = body_b.aabb;

            // check all PointMasses on bodyA for collision against bodyB.  if there is a collision, return detailed info.
            CollisionInfo infoAway = new CollisionInfo();
            CollisionInfo infoSame = new CollisionInfo();

            for (int i = 0; i < bApmCount; i++)
            {
                Vector2 pt = body_a.pointmass_list[i].position;

                // early out - if this point is outside the bounding box for bodyB, skip it!
                if (!boxB.Contains(pt.X, pt.Y))
                {
                    continue;
                }

                // early out - if this point is not inside bodyB, skip it!
                if (!body_b.Contains(ref pt))
                {
                    continue;
                }

                int prevPt = (i > 0) ? i - 1 : bApmCount - 1;
                int nextPt = (i < bApmCount - 1) ? i + 1 : 0;

                Vector2 prev = body_a.pointmass_list[prevPt].position;
                Vector2 next = body_a.pointmass_list[nextPt].position;

                // now get the normal for this point. (NOT A UNIT VECTOR)
                Vector2 fromPrev = new Vector2();
                fromPrev.X = pt.X - prev.X;
                fromPrev.Y = pt.Y - prev.Y;

                Vector2 toNext = new Vector2();
                toNext.X = next.X - pt.X;
                toNext.Y = next.Y - pt.Y;

                Vector2 ptNorm = new Vector2();
                ptNorm.X = fromPrev.X + toNext.X;
                ptNorm.Y = fromPrev.Y + toNext.Y;
                VectorHelper.Perpendicular(ref ptNorm);

                // this point is inside the other body.  now check if the edges on either side intersect with and edges on bodyB.
                float closestAway = 100000.0f;
                float closestSame = 100000.0f;

                infoAway.Clear();
                infoAway.body_a      = body_a;
                infoAway.pointmass_a = body_a.pointmass_list[i];
                infoAway.body_b      = body_b;

                infoSame.Clear();
                infoSame.body_a      = body_a;
                infoSame.pointmass_a = body_a.pointmass_list[i];
                infoSame.body_b      = body_b;

                bool found = false;

                int b1 = 0;
                int b2 = 1;
                for (int j = 0; j < bBpmCount; j++)
                {
                    Vector2 hitPt;
                    Vector2 norm;
                    float   edgeD;

                    b1 = j;

                    if (j < bBpmCount - 1)
                    {
                        b2 = j + 1;
                    }
                    else
                    {
                        b2 = 0;
                    }

                    Vector2 pt1 = body_b.pointmass_list[b1].position;
                    Vector2 pt2 = body_b.pointmass_list[b2].position;

                    // quick test of distance to each point on the edge, if both are greater than current mins, we can skip!
                    float distToA = ((pt1.X - pt.X) * (pt1.X - pt.X)) + ((pt1.Y - pt.Y) * (pt1.Y - pt.Y));
                    float distToB = ((pt2.X - pt.X) * (pt2.X - pt.X)) + ((pt2.Y - pt.Y) * (pt2.Y - pt.Y));


                    if ((distToA > closestAway) && (distToA > closestSame) && (distToB > closestAway) && (distToB > closestSame))
                    {
                        continue;
                    }

                    // test against this edge.
                    float dist = body_b.GetClosestPointOnEdgeSquared(pt, j, out hitPt, out norm, out edgeD);

                    // only perform the check if the normal for this edge is facing AWAY from the point normal.
                    float dot;
                    Vector2.Dot(ref ptNorm, ref norm, out dot);
                    if (dot <= 0f)
                    {
                        if (dist < closestAway)
                        {
                            closestAway            = dist;
                            infoAway.pointmass_b   = body_b.pointmass_list[b1];
                            infoAway.pointmass_c   = body_b.pointmass_list[b2];
                            infoAway.edge_distance = edgeD;
                            infoAway.point         = hitPt;
                            infoAway.normal        = norm;
                            infoAway.penetration   = dist;
                            found = true;
                        }
                    }
                    else
                    {
                        if (dist < closestSame)
                        {
                            closestSame            = dist;
                            infoSame.pointmass_b   = body_b.pointmass_list[b1];
                            infoSame.pointmass_c   = body_b.pointmass_list[b2];
                            infoSame.edge_distance = edgeD;
                            infoSame.point         = hitPt;
                            infoSame.normal        = norm;
                            infoSame.penetration   = dist;
                        }
                    }
                }

                // we've checked all edges on BodyB.
                if ((found) && (closestAway > 0.3f) && (closestSame < closestAway))
                {
                    infoSame.penetration = (float)Math.Sqrt(infoSame.penetration);
                    data.Add(infoSame);
                }
                else
                {
                    infoAway.penetration = (float)Math.Sqrt(infoAway.penetration);
                    data.Add(infoAway);
                }
            }

            return(data);
        }
예제 #3
0
        private void RotateShape(double elapsed)
        {
            // find the average angle of all of the masses.
            float angle         = 0;
            int   originalSign  = 1;
            float originalAngle = 0;

            for (int i = 0; i < count; i++)
            {
                Vector2 baseNorm = new Vector2();
                baseNorm.X = base_shape.points[i].X;
                baseNorm.Y = base_shape.points[i].Y;
                Vector2.Normalize(ref baseNorm, out baseNorm);

                Vector2 curNorm = new Vector2();
                curNorm.X = pointmass_list[i].position.X - position.X;
                curNorm.Y = pointmass_list[i].position.Y - position.Y;
                Vector2.Normalize(ref curNorm, out curNorm);

                float dot;
                Vector2.Dot(ref baseNorm, ref curNorm, out dot);
                if (dot > 1.0f)
                {
                    dot = 1.0f;
                }
                if (dot < -1.0f)
                {
                    dot = -1.0f;
                }

                float thisAngle = (float)Math.Acos(dot);
                if (!VectorHelper.IsCCW(ref baseNorm, ref curNorm))
                {
                    thisAngle = -thisAngle;
                }

                if (i == 0)
                {
                    originalSign  = (thisAngle >= 0.0f) ? 1 : -1;
                    originalAngle = thisAngle;
                }
                else
                {
                    float diff     = (thisAngle - originalAngle);
                    int   thisSign = (thisAngle >= 0.0f) ? 1 : -1;

                    if ((Math.Abs(diff) > Math.PI) && (thisSign != originalSign))
                    {
                        thisAngle = (thisSign == -1) ? ((float)Math.PI + ((float)Math.PI + thisAngle)) : (((float)Math.PI - thisAngle) - (float)Math.PI);
                    }
                }

                angle += thisAngle;
            }

            angle = angle / count;

            // now calculate the derived Omega, based on change in angle over time.
            float angleChange = (angle - prev_angle);

            if (Math.Abs(angleChange) >= Math.PI)
            {
                if (angleChange < 0f)
                {
                    angleChange = angleChange + (float)(Math.PI * 2);
                }
                else
                {
                    angleChange = angleChange - (float)(Math.PI * 2);
                }
            }

            omega      = angleChange / (float)elapsed;
            prev_angle = angle;

            for (int i = 0; i < count; i++)
            {
                float x = base_shape.points[i].X * scale.X;
                float y = base_shape.points[i].Y * scale.Y;
                float c = (float)Math.Cos(angle);
                float s = (float)Math.Sin(angle);
                curr_shape.points[i].X = (c * x) - (s * y) + position.X;
                curr_shape.points[i].Y = (c * y) + (s * x) + position.Y;
            }
        }
예제 #4
0
        public float GetClosestPointOnEdgeSquared(Vector2 point, int edgeNum, out Vector2 hitPt, out Vector2 normal, out float edgeD)
        {
            hitPt   = new Vector2();
            hitPt.X = 0f;
            hitPt.Y = 0f;

            normal   = new Vector2();
            normal.X = 0f;
            normal.Y = 0f;

            edgeD = 0f;
            float dist = 0f;

            Vector2 ptA = pointmass_list[edgeNum].position;
            Vector2 ptB = new Vector2();

            if (edgeNum < (count - 1))
            {
                ptB = pointmass_list[edgeNum + 1].position;
            }
            else
            {
                ptB = pointmass_list[0].position;
            }

            Vector2 toP = new Vector2();

            toP.X = point.X - ptA.X;
            toP.Y = point.Y - ptA.Y;

            Vector2 E = new Vector2();

            E.X = ptB.X - ptA.X;
            E.Y = ptB.Y - ptA.Y;

            // get the length of the edge, and use that to normalize the vector.
            float edgeLength = (float)Math.Sqrt((E.X * E.X) + (E.Y * E.Y));

            if (edgeLength > 0.00001f)
            {
                E.X /= edgeLength;
                E.Y /= edgeLength;
            }

            // normal
            Vector2 n = new Vector2();

            VectorHelper.Perpendicular(ref E, ref n);

            // calculate the distance!
            float x;

            Vector2.Dot(ref toP, ref E, out x);
            if (x <= 0.0f)
            {
                // x is outside the line segment, distance is from pt to ptA.
                //dist = (pt - ptA).Length();
                Vector2.DistanceSquared(ref point, ref ptA, out dist);
                hitPt  = ptA;
                edgeD  = 0f;
                normal = n;
            }
            else if (x >= edgeLength)
            {
                // x is outside of the line segment, distance is from pt to ptB.
                //dist = (pt - ptB).Length();
                Vector2.DistanceSquared(ref point, ref ptB, out dist);
                hitPt  = ptB;
                edgeD  = 1f;
                normal = n;
            }
            else
            {
                // point lies somewhere on the line segment.
                Vector3 toP3 = new Vector3();
                toP3.X = toP.X;
                toP3.Y = toP.Y;

                Vector3 E3 = new Vector3();
                E3.X = E.X;
                E3.Y = E.Y;

                //dist = Math.Abs(Vector3.Cross(toP3, E3).Z);
                Vector3.Cross(ref toP3, ref E3, out E3);
                dist    = Math.Abs(E3.Z * E3.Z);
                hitPt.X = ptA.X + (E.X * x);
                hitPt.Y = ptA.Y + (E.Y * x);
                edgeD   = x / edgeLength;
                normal  = n;
            }

            return(dist);
        }
예제 #5
0
        public void Update(double elapsed)
        {
            if (!initialized)
            {
                Initialize();
            }

            penetration_count = 0;
            collision_list.Clear();

            for (int i = 0; i < body_list.Count; i++)
            {
                body_list[i].Update(elapsed);
                UpdateBitmask(body_list[i]);
            }

            // update chains
            for (int i = 0; i < chain_list.Count; i++)
            {
                chain_list[i].Update(elapsed);
            }

            // now check for collision.
            // inter-body collision!
            for (int i = 0; i < body_list.Count; i++)
            {
                for (int j = i + 1; j < body_list.Count; j++)
                {
                    if (body_list[i].is_static && body_list[j].is_static)
                    {
                        continue;
                    }

                    // grid-based early out.
                    if (((body_list[i].bitmaskx.mask & body_list[j].bitmaskx.mask) == 0) &&
                        ((body_list[i].bitmasky.mask & body_list[j].bitmasky.mask) == 0))
                    {
                        continue;
                    }

                    // broad-phase collision via AABB.
                    if (!(body_list[i].aabb).Intersects(ref (body_list[j].aabb)))
                    {
                        continue;
                    }

                    if (on_aabb_collision != null)
                    {
                        this.on_aabb_collision(body_list[i], body_list[j]);
                    }

                    // okay, the AABB's of these 2 are intersecting.  now check for collision of A against B.
                    collision_list.AddRange(Collision.Intersects(body_list[j], body_list[i]));
                    collision_list.AddRange(Collision.Intersects(body_list[i], body_list[j]));
                }
            }

            // now handle all collisions found during the update at once.
            // handle all collisions!
            for (int i = 0; i < collision_list.Count; i++)
            {
                CollisionInfo info = collision_list[i];

                PointMass A  = info.pointmass_a;
                PointMass B1 = info.pointmass_b;
                PointMass B2 = info.pointmass_c;

                if (on_collision != null)
                {
                    this.on_collision(info.body_a, info.body_b, info);
                }

                // velocity changes as a result of collision.
                Vector2 bVel = new Vector2();
                bVel.X = (B1.velocity.X + B2.velocity.X) * 0.5f;
                bVel.Y = (B1.velocity.Y + B2.velocity.Y) * 0.5f;

                Vector2 relVel = new Vector2();
                relVel.X = A.velocity.X - bVel.X;
                relVel.Y = A.velocity.Y - bVel.Y;

                float relDot;
                Vector2.Dot(ref relVel, ref info.normal, out relDot);

                if (on_penetration != null)
                {
                    if (info.penetration > penetration_threshold)
                    {
                        this.on_penetration(info.body_a, info.body_b);
                    }
                }

                if (info.penetration > 0.3f)
                {
                    penetration_count++;
                    continue;
                }

                float b1inf = 1.0f - info.edge_distance;
                float b2inf = info.edge_distance;

                float b2MassSum = ((float.IsPositiveInfinity(B1.mass)) || (float.IsPositiveInfinity(B2.mass))) ? float.PositiveInfinity : (B1.mass + B2.mass);

                float massSum = A.mass + b2MassSum;

                float Amove;
                float Bmove;
                if (float.IsPositiveInfinity(A.mass))
                {
                    Amove = 0f;
                    Bmove = (info.penetration) + 0.001f;
                }
                else if (float.IsPositiveInfinity(b2MassSum))
                {
                    Amove = (info.penetration) + 0.001f;
                    Bmove = 0f;
                }
                else
                {
                    Amove = (info.penetration * (b2MassSum / massSum));
                    Bmove = (info.penetration * (A.mass / massSum));
                }

                float B1move = Bmove * b1inf;
                float B2move = Bmove * b2inf;

                float AinvMass = (float.IsPositiveInfinity(A.mass)) ? 0f : 1f / A.mass;
                float BinvMass = (float.IsPositiveInfinity(b2MassSum)) ? 0f : 1f / b2MassSum;

                float   jDenom = AinvMass + BinvMass;
                Vector2 numV   = new Vector2();
                float   elas   = elasticity;
                numV.X = relVel.X * elas;
                numV.Y = relVel.Y * elas;

                float jNumerator;
                Vector2.Dot(ref numV, ref info.normal, out jNumerator);
                jNumerator = -jNumerator;

                float j = jNumerator / jDenom;

                if (!float.IsPositiveInfinity(A.mass))
                {
                    A.position.X += info.normal.X * Amove;
                    A.position.Y += info.normal.Y * Amove;
                }

                if (!float.IsPositiveInfinity(B1.mass))
                {
                    B1.position.X -= info.normal.X * B1move;
                    B1.position.Y -= info.normal.Y * B1move;
                }

                if (!float.IsPositiveInfinity(B2.mass))
                {
                    B2.position.X -= info.normal.X * B2move;
                    B2.position.Y -= info.normal.Y * B2move;
                }

                Vector2 tangent = new Vector2();
                VectorHelper.Perpendicular(ref info.normal, ref tangent);
                float fNumerator;
                Vector2.Dot(ref relVel, ref tangent, out fNumerator);
                fNumerator *= friction;
                float f = fNumerator / jDenom;

                // adjust velocity if relative velocity is moving toward each other.
                if (relDot <= 0.0001f)
                {
                    if (!float.IsPositiveInfinity(A.mass))
                    {
                        A.velocity.X += (info.normal.X * (j / A.mass)) - (tangent.X * (f / A.mass));
                        A.velocity.Y += (info.normal.Y * (j / A.mass)) - (tangent.Y * (f / A.mass));
                    }

                    if (!float.IsPositiveInfinity(b2MassSum))
                    {
                        B1.velocity.X -= (info.normal.X * (j / b2MassSum) * b1inf) - (tangent.X * (f / b2MassSum) * b1inf);
                        B1.velocity.Y -= (info.normal.Y * (j / b2MassSum) * b1inf) - (tangent.Y * (f / b2MassSum) * b1inf);
                    }

                    if (!float.IsPositiveInfinity(b2MassSum))
                    {
                        B2.velocity.X -= (info.normal.X * (j / b2MassSum) * b2inf) - (tangent.X * (f / b2MassSum) * b2inf);
                        B2.velocity.Y -= (info.normal.Y * (j / b2MassSum) * b2inf) - (tangent.Y * (f / b2MassSum) * b2inf);
                    }

                    //info.body_a.UpdateBodyPositionVelocityForce(elapsed);
                    // info.body_b.UpdateBodyPositionVelocityForce(elapsed);
                }
            }

            for (int i = 0; i < body_list.Count; i++)
            {
                body_list[i].UpdateBodyPositionVelocityForce(elapsed);
            }
        }