示例#1
0
        private void resolveSphereSphereCollision(ref SphereProperties cur, ref SphereProperties other)
        {
            // the coefficient of restitution (1 for perfect elastic pulse)
            float e = 0.5f;
            float j = 0.0f;

            // compute penetration
            float penetration = 2 * mSphereRadius - Vector3.Distance(cur.position, other.position);

            // resolve player collision based on their impulses
            Vector3 rOne      = Vector3.Normalize(other.position - cur.position);
            Vector3 rTwo      = -rOne;
            Vector3 colNormal = rOne;

            rOne *= mSphereRadius;
            rTwo *= mSphereRadius;

            // project positions out of collisions
            cur.position   += rTwo * penetration / 2.0f;
            other.position += rOne * penetration / 2.0f;

            // relativ velocity
            float relVelocity = Vector3.Dot(colNormal, cur.velocity - other.velocity);

            // intermediate computations
            Vector3 tmpOne = Vector3.Cross(rOne, colNormal);

            tmpOne = Vector3.Transform(tmpOne, cur.invBodyInertia);
            tmpOne = Vector3.Cross(tmpOne, rOne);

            Vector3 tmpTwo = Vector3.Cross(rTwo, colNormal);

            tmpTwo = Vector3.Transform(tmpTwo, other.invBodyInertia);
            tmpTwo = Vector3.Cross(tmpTwo, rTwo);

            // compute j
            j = (-(1.0f + e) * relVelocity) / ((1.0f / mMass) + (1.0f / mMass) + Vector3.Dot(colNormal, tmpOne) + Vector3.Dot(colNormal, tmpTwo));

            // compute J
            Vector3 J = j * colNormal;

            // apply the impulse to the linear velocities
            cur.velocity   += J / mMass;
            other.velocity -= J / mMass;

            // compute impulse torque
            Vector3 torqueImpulsTwo = Vector3.Cross(rTwo, -J);
            Vector3 torqueImpulsOne = Vector3.Cross(rOne, J);

            // apply the torque to the angular momentum
            cur.angularMomentum   += torqueImpulsOne;
            other.angularMomentum -= torqueImpulsTwo;
        }
示例#2
0
        /// <summary>
        /// resolves player-level collisions
        /// </summary>
        private void resolveArenaCollisions(ref SphereProperties sphere, Vector3 avgColPoint, Vector3 avgFaceNormal)
        {
            // average inverse collision direction
            Vector3 avgColVec = sphere.position - avgColPoint;

            // collision direction
            Vector3 r = Vector3.Normalize(-avgColVec);

            r *= mSphereRadius;

            // move sphere out of colliding state
            sphere.position += (-r - avgColVec);

            // the coefficient of restitution (1 for perfect elastic pulse)
            float e = 0.9f;

            // the relative velocity
            float relativeVelocity = Vector3.Dot(sphere.velocity, avgFaceNormal);

            // intermediate computations
            Vector3 tmp = Vector3.Cross(r, avgFaceNormal);

            tmp = Vector3.Transform(tmp, sphere.invWorldInertia);
            tmp = Vector3.Cross(tmp, r);

            // the impulse's length
            float j = (-(1 + e) * relativeVelocity) / ((1 / mMass) + Vector3.Dot(avgFaceNormal, tmp));

            // the actual impulse
            Vector3 J = avgFaceNormal * j;

            // apply the impulse to the linear velocity
            sphere.velocity += (J / mMass);

            // the impulse torque
            Vector3 torqueImpuls = Vector3.Cross(r, J);

            // apply the torque to the angular velocity
            sphere.angularMomentum += torqueImpuls;

            // initiate rotation by changing angular momentum based on velocity of particle in contact with arena
            Vector3 omega = Vector3.Transform(sphere.angularMomentum, sphere.invBodyInertia);
            Vector3 collisionPointVelocity = Vector3.Cross(omega, r);

            // velocity difference of negative particle velocity and center of mass velocity
            Vector3 velocityDifference = collisionPointVelocity + sphere.velocity;

            // adjust angular momentum such that the velocity of the colliding particle equals the velocity of the center of mass
            sphere.angularMomentum = -Vector3.Cross(r, collisionPointVelocity - velocityDifference);
        }
示例#3
0
        /// <summary>
        /// Adds a new sphere to the simulator
        /// </summary>
        /// <param name="position">Location of the new sphere</param>
        /// <param name="velocity">Velocity vector of the new sphere</param>
        public void addSphere(Vector3 position, Vector3 velocity)
        {
            SphereProperties newSphere = new SphereProperties();

            newSphere.position        = position;
            newSphere.velocity        = velocity;
            newSphere.angularMomentum = Vector3.Zero;
            newSphere.rotation        = Quaternion.Identity;
            newSphere.invBodyInertia  = Matrix.Identity;
            newSphere.invWorldInertia = Matrix.Identity;

            mSimulatedSpheres.AddLast(newSphere);
            mSimulatedSpheresNextStep.AddLast(newSphere);
        }
示例#4
0
        private void handleSphereSphereCollisions()
        {
            // now handle sphere-sphere collisions
            LinkedListNode <SphereProperties> curSimulatedSphere  = mSimulatedSpheres.First;
            LinkedListNode <SphereProperties> nextSimulatedSphere = mSimulatedSpheresNextStep.First;

            float radiusSquared = mSphereRadius * mSphereRadius;

            // for each (unordered) sphere-sphere pair:
            while (curSimulatedSphere != null)
            {
                SphereProperties curSphere = curSimulatedSphere.Value;

                LinkedListNode <SphereProperties> otherSphere = curSimulatedSphere.Next;
                while (otherSphere != null)
                {
                    // spheres collide? call resolveSphereSphereCollision()
                    if (Vector3.Distance(curSphere.position, otherSphere.Value.position) <= 2 * mSphereRadius)
                    {
                        SphereProperties other = otherSphere.Value;
                        resolveSphereSphereCollision(ref curSphere, ref other);

                        // update the other sphere
                        otherSphere.Value = other;
                    }

                    otherSphere = otherSphere.Next;
                }

                // update current sphere
                nextSimulatedSphere.Value = curSphere;

                // advance to next sphere
                curSimulatedSphere  = curSimulatedSphere.Next;
                nextSimulatedSphere = nextSimulatedSphere.Next;
            }

            // swap lists (update all spheres at once)
            LinkedList <SphereProperties> tmp = mSimulatedSpheres;

            mSimulatedSpheres         = mSimulatedSpheresNextStep;
            mSimulatedSpheresNextStep = tmp;
        }
示例#5
0
        private void handleArenaCollisions(ref SphereProperties sphere)//(GameTime gameTime)
        {
            // compute collisions with level and store them in mCollisionPoints
            mTimePhysics.Stop();
            mTimeCollisions.Start();

            LinkedList <Face>    arenaCollidingFaces  = new LinkedList <CollidableModel.Face>();
            LinkedList <Vector3> arenaCollisionPoints = new LinkedList <Vector3>();

            mBspTree.collisions(new BoundingSphere(sphere.position, mSphereRadius), arenaCollidingFaces, arenaCollisionPoints);

            mNumCollisionTests++;
            mTimeCollisions.Stop();
            mTimePhysics.Start();

            // add collisions to colliding faces
            foreach (Face f in arenaCollidingFaces)
            {
                if (mCollidingFaces.ContainsKey(f))
                {
                    mCollidingFaces[f]++;
                }
                else
                {
                    mCollidingFaces.Add(f, 1);
                }
            }

            // are there any collisions?
            if (arenaCollisionPoints.Count != 0)
            {
                // compute average of all collision points
                Vector3 avgColPoint     = Vector3.Zero;
                Vector3 avgFaceNormal   = Vector3.Zero;
                Vector3 closestNormal   = Vector3.Zero;
                float   closestDistance = float.MaxValue;

                LinkedListNode <Vector3> curPoint = arenaCollisionPoints.First;
                LinkedListNode <Face>    curFace  = arenaCollidingFaces.First;
                while (curPoint != null)
                {
                    avgFaceNormal += curFace.Value.faceNormal;
                    avgColPoint   += curPoint.Value;

                    // update closest normal
                    float d = (sphere.position - curPoint.Value).LengthSquared();
                    if (d < closestDistance)
                    {
                        closestDistance = d;
                        closestNormal   = curFace.Value.faceNormal;
                    }
                    curPoint = curPoint.Next;
                    curFace  = curFace.Next;
                }

                // this can happen at very thin walls for example
                if (avgFaceNormal == Vector3.Zero)
                {
                    avgFaceNormal = closestNormal;
                }

                // normalize
                avgFaceNormal.Normalize();
                avgColPoint /= arenaCollisionPoints.Count;

                // --- resolve collision ------
                resolveArenaCollisions(ref sphere, avgColPoint, avgFaceNormal);
            }
        }
示例#6
0
        /// <summary>
        /// Advance the simulator by one timestep
        /// </summary>
        /// <param name="time"></param>
        public void update(GameTime time)
        {
            // restest counters and stop watches
            mNumCollisionTests = 0;
            mTimePhysics.Reset();
            mTimeCollisions.Reset();

            mTimePhysics.Start();
            mCollidingFaces.Clear();

            // time delta
            float dt = time.ElapsedGameTime.Milliseconds * timeScale / numSubSteps;

            // for each substep
            for (int step = 0; step < numSubSteps; step++)
            {
                // iterate through all spheres
                LinkedListNode <SphereProperties> curSimulatedSphere  = mSimulatedSpheres.First;
                LinkedListNode <SphereProperties> nextSimulatedSphere = mSimulatedSpheresNextStep.First;

                while (curSimulatedSphere != null)
                {
                    // copy current sphere properties
                    SphereProperties sphere = curSimulatedSphere.Value;

                    // compute forces
                    Vector3 forces = -mLinearFriction * curSimulatedSphere.Value.velocity;
                    forces += mGravity;

                    // compute acceleration
                    Vector3 acceleration = forces / mMass;

                    // integrate (keep it simple: just use euler)
                    sphere.velocity += acceleration * dt;
                    sphere.position += sphere.velocity * dt;

                    // compute new angular momentum (damping)
                    sphere.angularMomentum *= mAngularDamping;

                    // integrate angular momentum
                    Matrix rot = Matrix.CreateFromQuaternion(curSimulatedSphere.Value.rotation);
                    sphere.invWorldInertia = rot * sphere.invBodyInertia * Matrix.Transpose(rot);

                    // compute angular velocity
                    Vector3 omega = Vector3.Transform(sphere.angularMomentum, sphere.invWorldInertia);

                    // integrate rotational part into quaternion
                    Quaternion quatRotDot = new Quaternion(omega, 0.0f) * sphere.rotation;
                    quatRotDot *= -0.5f;
                    quatRotDot *= dt;

                    quatRotDot += sphere.rotation;
                    quatRotDot.Normalize();

                    sphere.rotation = quatRotDot;


                    // check for arena collisions and resolve them
                    handleArenaCollisions(ref sphere);

                    // copy new sphere properties to updated buffer
                    nextSimulatedSphere.Value = sphere;

                    // remove spheres below arena
                    LinkedListNode <SphereProperties> oldNode  = curSimulatedSphere;
                    LinkedListNode <SphereProperties> oldNode2 = nextSimulatedSphere;

                    curSimulatedSphere  = curSimulatedSphere.Next;
                    nextSimulatedSphere = nextSimulatedSphere.Next;

                    if (sphere.position.Y < -200)
                    {
                        mSimulatedSpheres.Remove(oldNode);
                        mSimulatedSpheresNextStep.Remove(oldNode2);
                    }
                }

                // swap lists (update all spheres at once)
                LinkedList <SphereProperties> tmp = mSimulatedSpheres;
                mSimulatedSpheres         = mSimulatedSpheresNextStep;
                mSimulatedSpheresNextStep = tmp;

                // compute and resolve sphere-sphere collisions
                handleSphereSphereCollisions();
            }

            // stop stopwatch
            mTimePhysics.Stop();
        }
        private void resolveSphereSphereCollision(ref SphereProperties cur, ref SphereProperties other)
        {
            // the coefficient of restitution (1 for perfect elastic pulse)
            float e = 0.5f;
            float j = 0.0f;

            // compute penetration
            float penetration = 2*mSphereRadius - Vector3.Distance( cur.position, other.position );

            // resolve player collision based on their impulses
            Vector3 rOne = Vector3.Normalize(other.position - cur.position);
            Vector3 rTwo = -rOne;
            Vector3 colNormal = rOne;

            rOne *= mSphereRadius;
            rTwo *= mSphereRadius;

            // project positions out of collisions
            cur.position += rTwo * penetration / 2.0f;
            other.position += rOne * penetration / 2.0f;

            // relativ velocity
            float relVelocity = Vector3.Dot(colNormal, cur.velocity - other.velocity);

            // intermediate computations
            Vector3 tmpOne = Vector3.Cross(rOne, colNormal);
            tmpOne = Vector3.Transform(tmpOne, cur.invBodyInertia);
            tmpOne = Vector3.Cross(tmpOne, rOne);

            Vector3 tmpTwo = Vector3.Cross(rTwo, colNormal);
            tmpTwo = Vector3.Transform(tmpTwo, other.invBodyInertia);
            tmpTwo = Vector3.Cross(tmpTwo, rTwo);

            // compute j
            j = (-(1.0f + e) * relVelocity) / ((1.0f / mMass) + (1.0f / mMass) + Vector3.Dot(colNormal, tmpOne) + Vector3.Dot(colNormal, tmpTwo));

            // compute J
            Vector3 J = j * colNormal;

            // apply the impulse to the linear velocities
            cur.velocity += J / mMass;
            other.velocity -= J / mMass;

            // compute impulse torque
            Vector3 torqueImpulsTwo = Vector3.Cross(rTwo, -J);
            Vector3 torqueImpulsOne = Vector3.Cross(rOne, J);

            // apply the torque to the angular momentum
            cur.angularMomentum += torqueImpulsOne;
            other.angularMomentum -= torqueImpulsTwo;
        }
        /// <summary>
        /// resolves player-level collisions
        /// </summary>
        private void resolveArenaCollisions(ref SphereProperties sphere, Vector3 avgColPoint, Vector3 avgFaceNormal)
        {
            // average inverse collision direction
            Vector3 avgColVec = sphere.position - avgColPoint;

            // collision direction
            Vector3 r = Vector3.Normalize(-avgColVec);
            r *= mSphereRadius;

            // move sphere out of colliding state
            sphere.position += (-r - avgColVec);

            // the coefficient of restitution (1 for perfect elastic pulse)
            float e = 0.9f;

            // the relative velocity
            float relativeVelocity = Vector3.Dot(sphere.velocity, avgFaceNormal);

            // intermediate computations
            Vector3 tmp = Vector3.Cross(r, avgFaceNormal);
            tmp = Vector3.Transform(tmp, sphere.invWorldInertia);
            tmp = Vector3.Cross(tmp, r);

            // the impulse's length
            float j = (-(1 + e) * relativeVelocity) / ((1 / mMass) + Vector3.Dot(avgFaceNormal, tmp));

            // the actual impulse
            Vector3 J = avgFaceNormal * j;

            // apply the impulse to the linear velocity
            sphere.velocity += (J / mMass);

            // the impulse torque
            Vector3 torqueImpuls = Vector3.Cross(r, J);

            // apply the torque to the angular velocity
            sphere.angularMomentum += torqueImpuls;

            // initiate rotation by changing angular momentum based on velocity of particle in contact with arena
            Vector3 omega = Vector3.Transform(sphere.angularMomentum, sphere.invBodyInertia);
            Vector3 collisionPointVelocity = Vector3.Cross(omega, r);

            // velocity difference of negative particle velocity and center of mass velocity
            Vector3 velocityDifference = collisionPointVelocity + sphere.velocity;

            // adjust angular momentum such that the velocity of the colliding particle equals the velocity of the center of mass
            sphere.angularMomentum = -Vector3.Cross(r, collisionPointVelocity - velocityDifference);
        }
        //(GameTime gameTime)
        private void handleArenaCollisions(ref SphereProperties sphere)
        {
            // compute collisions with level and store them in mCollisionPoints
            mTimePhysics.Stop();
            mTimeCollisions.Start();

            LinkedList<Face> arenaCollidingFaces = new LinkedList<CollidableModel.Face>();
            LinkedList<Vector3> arenaCollisionPoints = new LinkedList<Vector3>();

            mBspTree.collisions(new BoundingSphere(sphere.position, mSphereRadius), arenaCollidingFaces, arenaCollisionPoints);

             mNumCollisionTests++;
            mTimeCollisions.Stop();
            mTimePhysics.Start();

            // add collisions to colliding faces
            foreach (Face f in arenaCollidingFaces)
            {

                if (mCollidingFaces.ContainsKey(f))
                    mCollidingFaces[f]++;
                else
                    mCollidingFaces.Add(f, 1);

            }

            // are there any collisions?
            if (arenaCollisionPoints.Count != 0)
            {
                // compute average of all collision points
                Vector3 avgColPoint = Vector3.Zero;
                Vector3 avgFaceNormal = Vector3.Zero;
                Vector3 closestNormal = Vector3.Zero;
                float closestDistance = float.MaxValue;

                LinkedListNode<Vector3> curPoint = arenaCollisionPoints.First;
                LinkedListNode<Face> curFace = arenaCollidingFaces.First;
                while (curPoint != null)
                {
                    avgFaceNormal += curFace.Value.faceNormal;
                    avgColPoint += curPoint.Value;

                    // update closest normal
                    float d = (sphere.position - curPoint.Value).LengthSquared();
                    if (d < closestDistance)
                    {
                        closestDistance = d;
                        closestNormal = curFace.Value.faceNormal;
                    }
                    curPoint = curPoint.Next;
                    curFace = curFace.Next;
                }

                // this can happen at very thin walls for example
                if (avgFaceNormal == Vector3.Zero)
                    avgFaceNormal = closestNormal;

                // normalize
                avgFaceNormal.Normalize();
                avgColPoint /= arenaCollisionPoints.Count;

                // --- resolve collision ------
                resolveArenaCollisions(ref sphere, avgColPoint, avgFaceNormal);
            }
        }
        /// <summary>
        /// Adds a new sphere to the simulator
        /// </summary>
        /// <param name="position">Location of the new sphere</param>
        /// <param name="velocity">Velocity vector of the new sphere</param>
        public void addSphere(Vector3 position, Vector3 velocity)
        {
            SphereProperties newSphere = new SphereProperties();
            newSphere.position = position;
            newSphere.velocity = velocity;
            newSphere.angularMomentum = Vector3.Zero;
            newSphere.rotation = Quaternion.Identity;
            newSphere.invBodyInertia = Matrix.Identity;
            newSphere.invWorldInertia = Matrix.Identity;

            mSimulatedSpheres.AddLast(newSphere);
            mSimulatedSpheresNextStep.AddLast(newSphere);
        }