void IPositionUpdateable.PreUpdatePosition(float dt) { System.Numerics.Vector3 increment; Vector3Ex.Multiply(ref angularVelocity, dt * .5f, out increment); var multiplier = new System.Numerics.Quaternion(increment.X, increment.Y, increment.Z, 0); QuaternionEx.Multiply(ref multiplier, ref orientation, out multiplier); QuaternionEx.Add(ref orientation, ref multiplier, out orientation); orientation = System.Numerics.Quaternion.Normalize(orientation); Matrix3x3.CreateFromQuaternion(ref orientation, out orientationMatrix); //Only do the linear motion if this object doesn't obey CCD. if (PositionUpdateMode == PositionUpdateMode.Discrete) { Vector3Ex.Multiply(ref linearVelocity, dt, out increment); Vector3Ex.Add(ref position, ref increment, out position); collisionInformation.UpdateWorldTransform(ref position, ref orientation); //The position update is complete if this is a discretely updated object. if (PositionUpdated != null) { PositionUpdated(this); } } MathChecker.Validate(linearVelocity); MathChecker.Validate(angularVelocity); MathChecker.Validate(position); MathChecker.Validate(orientation); #if CONSERVE MathChecker.Validate(angularMomentum); #endif }
/// <summary> /// Integrates the position and orientation of the bone forward based upon the current linear and angular velocity. /// </summary> internal void UpdatePosition() { //Update the position based on the linear velocity. Vector3Ex.Add(ref Position, ref linearVelocity, out Position); //Update the orientation based on the angular velocity. System.Numerics.Vector3 increment; Vector3Ex.Multiply(ref angularVelocity, .5f, out increment); var multiplier = new System.Numerics.Quaternion(increment.X, increment.Y, increment.Z, 0); QuaternionEx.Multiply(ref multiplier, ref Orientation, out multiplier); QuaternionEx.Add(ref Orientation, ref multiplier, out Orientation); Orientation = System.Numerics.Quaternion.Normalize(Orientation); //Eliminate any latent velocity in the bone to prevent unwanted simulation feedback. //This is the only thing conceptually separating this "IK" solver from the regular dynamics loop in BEPUphysics. //(Well, that and the whole lack of collision detection...) linearVelocity = new System.Numerics.Vector3(); angularVelocity = new System.Numerics.Vector3(); //Note: Unlike a regular dynamics simulation, we do not include any 'dt' parameter in the above integration. //Setting the velocity to 0 every update means that no more than a single iteration's worth of velocity accumulates. //Since the softness of constraints already varies with the time step and bones never accelerate for more than one frame, //scaling the velocity for position integration actually turns out generally worse. //This is not a rigorously justifiable approach, but this isn't a regular dynamic simulation anyway. }