private void IntegrateCallback(object obj) { RigidBody body = obj as RigidBody; TSVector temp; TSVector.Multiply(ref body.linearVelocity, timestep, out temp); TSVector.Add(ref temp, ref body.position, out body.position); if (!(body.isParticle)) { //exponential map TSVector axis; FP angle = body.angularVelocity.magnitude; FP halfTimeStep = FP.Half * timestep; FP halfTimeStepAngle = halfTimeStep * angle; if (angle < FP.EN3) { // use Taylor's expansions of sync function // axis = body.angularVelocity * (FP.Half * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle); //TSVector.Multiply(ref body.angularVelocity, (halfTimeStep - (timestep * timestep * timestep) * (2082 * FP.EN6) * angle * angle), out axis); TSVector.Multiply(ref body.angularVelocity, halfTimeStep, out axis); } else { // sync(fAngle) = sin(c*fAngle)/t TSVector.Multiply(ref body.angularVelocity, (FP.Sin(halfTimeStepAngle) / angle), out axis); } TSQuaternion dorn = new TSQuaternion(axis.x, axis.y, axis.z, FP.Cos(halfTimeStepAngle)); TSQuaternion ornA; TSQuaternion.CreateFromMatrix(ref body.orientation, out ornA); TSQuaternion.Multiply(ref dorn, ref ornA, out dorn); dorn.Normalize(); TSMatrix.CreateFromQuaternion(ref dorn, out body.orientation); } body.linearVelocity /= (1 + timestep * body.linearDrag); body.angularVelocity /= (1 + timestep * body.angularDrag); /*if ((body.Damping & RigidBody.DampingType.Linear) != 0) * TSVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity); * * if ((body.Damping & RigidBody.DampingType.Angular) != 0) * TSVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);*/ body.Update(); if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts) { body.SweptExpandBoundingBox(timestep); } }
/// <summary> /// Constructor. /// </summary> /// <param name="body1">The first body which should get constrained. Can be null.</param> /// <param name="body2">The second body which should get constrained. Can be null.</param> public Constraint(RigidBody body1, RigidBody body2) { this.body1 = body1; this.body2 = body2; instanceCount++; instance = instanceCount; // calling body.update does not hurt // if the user set orientations all // inverse orientations etc. get also // recalculated. if (body1 != null) { body1.Update(); } if (body2 != null) { body2.Update(); } }