Ejemplo n.º 1
0
        ///<summry>
        /// Fills the given contact structure with the contact needed
        /// to keep the cable from over-extending.
        ///</summry>
        public override bool Check(Contact contact)
        {
            // Find the length of the cable
            float length = currentLength();

            // Check if we're over-extended
            if (length < maxLength)
            {
                return false;
            }

            // Otherwise return the contact
            // Calculate the normal
            Vector3 normal = body[1].Position - body[0].Position;
            normal.Normalize();
            contact.ContactNormal = normal;

            contact.Penetration = length - maxLength;
            contact.restitution = restitution;

            if (body[0].HasFiniteMass)
                contact.ContactPoint = body[1].Position;// +body[1].getHalfSize();

            if (body[1].HasFiniteMass)
                contact.ContactPoint = body[0].Position;// +body[0].getHalfSize();

            return true;
        }
Ejemplo n.º 2
0
        public bool addContact(Contact contact)
        {
            // Calculate the position of each connection point in world coordinates
            Vector3 onePosWorld = body[0].GetPointInWorldSpace(position[0]);
            Vector3 twoPosWorld = body[1].GetPointInWorldSpace(position[1]);

            // Calculate the length of the joint
            Vector3 oneToTwo = twoPosWorld - onePosWorld;
            Vector3 normal = oneToTwo;
            normal = Vector3.Normalize(normal);

            float length = oneToTwo.Length();

            // Check if it is violated
            if (length > constant)
            {
                contact.body[0] = body[0];

                contact.body[1] = body[1];
                contact.ContactNormal = normal;
                contact.ContactPoint = (onePosWorld + twoPosWorld) * 0.5f;
                contact.Penetration = length - constant;
                //TODO add friction and restitution to contactdata
                contact.friction = 1.0f;
                contact.restitution = 0;
                return true;
            }

            return false;
        }
Ejemplo n.º 3
0
 public override bool generateContacts(Collidable other, Contact contact)
 {
     if (other as Box != null)
     {
         return contact.BoxAndBox();
     }
     else if (other as Sphere != null)
     {
         return contact.SphereAndBox();
     }
     return false;
 }
Ejemplo n.º 4
0
 public ContactData generateContacts(Collidable other)
 {
     Contact contact = new Contact(other, this.plane);
     if (other as Sphere != null)
     {
         contact.SphereAndPlane();
     }
     if (other as Box != null)
     {
         contact.BoxAndHalfSpace();
     }
     return contact.GetContactData();
 }
Ejemplo n.º 5
0
        /*
        protected override void updateBounding()
        {
            //TODO add update logic
        }

        public override Contact generateContacts(Collidable other)
        {
            Contact contactData = null;

            return contactData;
        }
        */
        public bool generateContacts(Collidable other, Contact contact)
        {
            //TODO FixME
            return false;
            if (other as Sphere != null)
            {
                contact.SphereAndPlane();
            }
            if (other as Box != null)
            {
                contact.BoxAndHalfSpace();
            }
        }
Ejemplo n.º 6
0
 public override ContactData generateContacts(Collidable other)
 {
     Contact contact = null;
     contact = new Contact(this, other);
     if (other as Box != null)
     {
         contact.BoxAndBox();
     }
     if (other as Sphere != null)
     {
         contact.SphereAndBox();
     }
     return contact.GetContactData();
 }
Ejemplo n.º 7
0
 public void FindPotentialCollisionsWith(List<Contact> Potentials, BVHNode other)
 {
     if (!CollidesWith(other))
     {
         if (!this.isLeaf())
             this.FindPotentialCollisions(Potentials);
         if (!other.isLeaf())
             other.FindPotentialCollisions(Potentials);
         return;
     }
     if (this.isLeaf() && other.isLeaf())
     {
         Contact temp = new Contact(this.Body, other.Body);
         if (!temp.BothFixed())
             Potentials.Add(temp);
     }
     else if (other.isLeaf() && !this.isLeaf())
     {
         this.Children[0].FindPotentialCollisionsWith(Potentials, other);
         this.Children[1].FindPotentialCollisionsWith(Potentials, other);
         this.FindPotentialCollisions(Potentials);
     }
     else if (this.isLeaf() && !other.isLeaf())
     {
         other.Children[0].FindPotentialCollisionsWith(Potentials, this);
         other.Children[1].FindPotentialCollisionsWith(Potentials, this);
     }
     else
     {
         other.Children[0].FindPotentialCollisionsWith(Potentials, this.Children[0]);
         other.Children[0].FindPotentialCollisionsWith(Potentials, this.Children[1]);
         other.Children[1].FindPotentialCollisionsWith(Potentials, this.Children[0]);
         other.Children[1].FindPotentialCollisionsWith(Potentials, this.Children[1]);
         this.FindPotentialCollisions(Potentials);
         other.FindPotentialCollisions(Potentials);
     }
 }
Ejemplo n.º 8
0
        private Vector3 CalculateFrictionlessImpulse(Contact contactData, Matrix3[] inverseInertiaTensor)
        {
            Body one = contactData.body[0];
            Body two = contactData.body[1];
            Vector3 impulseContact;

            // Build a vector that shows the change in velocity in
            // world space for a unit impulse in the direction of the contact
            // normal.
            Vector3 torquePerUnitImpulse1 = Vector3.Cross(contactData.relativeContactPosition[0], contactData.ContactNormal);
            Vector3 rotationPerUnitImpluse1 = inverseInertiaTensor[0].transform(torquePerUnitImpulse1);
            Vector3 VelocityPerUnitImpulse1 = Vector3.Cross(rotationPerUnitImpluse1, contactData.relativeContactPosition[0]);

            // Work out the change in velocity in contact coordiantes.
            float deltaVelocity = Vector3.Dot(VelocityPerUnitImpulse1, contactData.ContactNormal);

            // Add the linear component of velocity change
            deltaVelocity += one.InverseMass;

            // Check if we need to the second body's data
            if (two != null)
            {
                // Go through the same transformation sequence again
                Vector3 torquePerUnitImpulse2 = Vector3.Cross(contactData.relativeContactPosition[1], contactData.ContactNormal);
                Vector3 rotationPerUnitImpluse2 = inverseInertiaTensor[1].transform(torquePerUnitImpulse2);
                Vector3 VelocityPerUnitImpulse2 = Vector3.Cross(rotationPerUnitImpluse2, contactData.relativeContactPosition[1]);

                // Add the change in velocity due to rotation
                deltaVelocity += Vector3.Dot(VelocityPerUnitImpulse2, contactData.ContactNormal);

                // Add the change in velocity due to linear motion
                deltaVelocity += two.InverseMass;
            }

            // Calculate the required size of the impulse
            impulseContact.X = contactData.desiredDeltaVelocity / deltaVelocity;
            impulseContact.Y = 0;
            impulseContact.Z = 0;
            return impulseContact;
        }
Ejemplo n.º 9
0
        private Vector3 CalculateFrictionImpulse(Contact contactData, Matrix3[] inverseInertiaTensor)
        {
            Body one = contactData.body[0];
            Body two = contactData.body[1];
            Vector3 impulseContact;
            float inverseMass = one.InverseMass;

            // The equivalent of a cross product in matrices is multiplication
            // by a skew symmetric matrix - we build the matrix for converting
            // between linear and angular quantities.
            Matrix3 impulseToTorque = new Matrix3();
            impulseToTorque.setSkewSymmetric(contactData.relativeContactPosition[0]);

            // Build the matrix to convert contact impulse to change in velocity
            // in world coordinates.
            Matrix3 deltaVelWorld = impulseToTorque;
            deltaVelWorld *= inverseInertiaTensor[0];
            deltaVelWorld *= impulseToTorque;
            deltaVelWorld *= -1;

            // Check if we need to add body two's data
            if (two != null)
            {
                // Set the cross product matrix
                impulseToTorque.setSkewSymmetric(contactData.relativeContactPosition[1]);

                // Calculate the velocity change matrix
                Matrix3 deltaVelWorldTwo = impulseToTorque;
                deltaVelWorldTwo *= inverseInertiaTensor[1];
                deltaVelWorldTwo *= impulseToTorque;
                deltaVelWorldTwo *= -1;

                // Add to the total delta velocity.
                deltaVelWorld += deltaVelWorldTwo;

                // Add to the inverse mass
                inverseMass += two.InverseMass;
            }

            // Do a change of basis to convert into contact coordinates.
            Matrix3 deltaVelocity = contactData.ContactToWorld.transpose();
            deltaVelocity *= deltaVelWorld;
            deltaVelocity *= contactData.ContactToWorld;

            // Add in the linear velocity change
            deltaVelocity.data[0] += inverseMass;
            deltaVelocity.data[4] += inverseMass;
            deltaVelocity.data[8] += inverseMass;

            // Invert to get the impulse needed per unit velocity
            Matrix3 impulseMatrix = deltaVelocity.inverse();

            // Find the velocities that will be removed
            Vector3 velKill = new Vector3(contactData.desiredDeltaVelocity,
                -contactData.contactVelocity.Y,
                -contactData.contactVelocity.Z);

            // Find the impulse to kill target velocities
            impulseContact = impulseMatrix.transform(velKill);

            // Check for exceeding friction
            float planarImpulse = (float)Math.Sqrt(Convert.ToDouble(impulseContact.Y * impulseContact.Y + impulseContact.Z * impulseContact.Z));

            if ((planarImpulse > impulseContact.X * contactData.friction) && (planarImpulse != 0))
            {
                // We need to use dynamic friction
                impulseContact.Y /= planarImpulse;
                impulseContact.Z /= planarImpulse;

                impulseContact.X = deltaVelocity.data[0] +
                    deltaVelocity.data[1] * contactData.friction * impulseContact.Y +
                    deltaVelocity.data[2] * contactData.friction * impulseContact.Z;
                impulseContact.X = contactData.desiredDeltaVelocity / impulseContact.X;
                impulseContact.Y *= contactData.friction * impulseContact.X;
                impulseContact.Z *= contactData.friction * impulseContact.X;
            }
            return impulseContact;
        }
Ejemplo n.º 10
0
        //LEVEL 1
        void calculateContactInformations(float duration)
        {
            // BoundingBox world = new BoundingBox();
            // TODO !! make real world
            // CollisionDetector collisionGenerator = new CollisionDetector(world, bodies);
            CollisionDetector collisionGenerator = new CollisionDetector(bodies,planes);
            this.contactDataList= collisionGenerator.ReDetect();
            //if (contactDataList.Count == 0) return;
            this.contactDataList.RemoveAll((Contact contact) => {
                return !contact.Check();
            });

            foreach (Link con in conductorList)
            {
                Contact temp = new Contact(con.body[0], con.body[1]);
                if (con.Check(temp))
                    contactDataList.Add(temp);
            }

            foreach (Joint joint in jointList)
            {
                Contact contact=new Contact(null,null);
                if (joint.addContact(contact))
                    contactDataList.Add(contact);
            }
        }
Ejemplo n.º 11
0
        public void ApplyVelocityChange(Contact contactData,out Vector3[] velocityChange,out Vector3[] rotationChange)
        {
            Body one = contactData.body[0];
            Body two = contactData.body[1];
            velocityChange = new Vector3[2];
            rotationChange = new Vector3[2];
            // Get hold of the inverse mass and inverse inertia tensor, both in
            // world coordinates.
            Matrix3[] inverseInertiaTensor = new Matrix3[2];
            inverseInertiaTensor[0] = one.InverseInertiaTensorWorld;
            if (two != null)
                inverseInertiaTensor[1] = two.InverseInertiaTensorWorld;

            // We will calculate the impulse for each contact axis
            Vector3 impulseContact;

            if (contactData.friction == 0.0f)
            {
                //ther is no friction
                impulseContact = CalculateFrictionlessImpulse(contactData, inverseInertiaTensor);
            }
            else
            {
                // Otherwise we may have impulses that aren't in the direction of the
                // contact, so we need the more complex version.
                impulseContact = CalculateFrictionImpulse(contactData, inverseInertiaTensor);
            }

            // Convert impulse to world coordinates
            Vector3 impulse = contactData.ContactToWorld.transform(impulseContact);

            // Split in the impulse into linear and rotational components
            Vector3 impulsiveTorqueOne = Vector3.Cross(contactData.relativeContactPosition[0],impulse);
            rotationChange[0] = inverseInertiaTensor[0].transform(impulsiveTorqueOne);

            velocityChange[0] = impulse * one.InverseMass;

            // Apply the changes
            one.AddVelocity(velocityChange[0]);
            one.Rotation += rotationChange[0];

            if (two != null)
            {
                // Work out body one's linear and angular changes
                Vector3 impulsiveTorqueTwo = Vector3.Cross(impulse,contactData.relativeContactPosition[1]);
                rotationChange[1] = inverseInertiaTensor[1].transform(impulsiveTorqueTwo);
                velocityChange[1] = -impulse * two.InverseMass;

                // And apply them.
                two.AddVelocity(velocityChange[1]);
                two.Rotation += rotationChange[1];
            }
        }
Ejemplo n.º 12
0
 public void AddContactData(Contact contactdata)
 {
     contactDataList.Add(contactdata);
 }
Ejemplo n.º 13
0
 ///<summary>
 /// Fills the given contact structure with the generated
 /// contact. The contact pointer should point to the first
 /// available contact in a contact array, where limit is the
 /// maximum number of contacts in the array that can be written
 /// to. The method returns the number of contacts that have
 /// been written.
 ///</summary>
 //public uint addContact(Contact contact);
 //public abstract uint addContact();
 public abstract bool Check(Contact contact);
Ejemplo n.º 14
0
        /**
         * Fills the given contact structure with the contact needed
         * to keep the rod from extending or compressing.
         */
        public override bool Check(Contact contact)
        {
            // Find the length of the rod
            float currentLen = currentLength();

            // Check if we're over-extended
            if (currentLen == length)
            {
                return false;
            }

            // Otherwise return the contact
            // Calculate the normal
            Vector3 normal = body[1].Position - body[0].Position;
            normal.Normalize();

            // The contact normal depends on whether we're extending or compressing
            if (currentLen > length)
            {
                contact.ContactNormal = normal;
                contact.Penetration = currentLen - length;
            }
            else
            {
                contact.ContactNormal = normal * -1;
                contact.Penetration = length - currentLen;
            }

            if (body[0].HasFiniteMass)
                contact.ContactPoint = body[1].Position;// +body[1].getHalfSize();

            if (body[1].HasFiniteMass)
                contact.ContactPoint = body[0].Position;// +body[0].getHalfSize();

            // Always use zero restitution (no bounciness)
            contact.restitution = 1;

            return true;
        }
Ejemplo n.º 15
0
 /// <summary>
 ///  generates contact information for this collidable body with another one 
 ///  and fill this info into an existing contact
 /// </summary>
 /// <param name="other"></param>
 /// <param name="contact">a contact to fill</param>
 /// <returns></returns>
 public abstract bool generateContacts(Collidable other,Contact contact);