public static TSQuaternion LerpUnclamped(TSQuaternion a, TSQuaternion b, FP t)
{
TSQuaternion result = TSQuaternion.Multiply(a, (1 - t)) + TSQuaternion.Multiply(b, t);
result.Normalize();
return(result);
}
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);
}
}
public static TSQuaternion FromToRotation(TSVector fromVector, TSVector toVector)
{
TSVector w = TSVector.Cross(fromVector, toVector);
TSQuaternion q = new TSQuaternion(w.x, w.y, w.z, TSVector.Dot(fromVector, toVector));
q.w += FP.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude);
q.Normalize();
return(q);
}
private void Integrate()
{
for (int index = 0, length = rigidBodies.Count; index < length; index++)
{
var body = rigidBodies[index];
if (body.isStatic || !body.IsActive)
{
continue;
}
body.position += body.linearVelocity * timestep;
if (!(body.isParticle))
{
//exponential map
TSVector axis;
FP angle = body.angularVelocity.magnitude;
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(body.angularVelocity, (FP.Half * timestep - (timestep * timestep * timestep) * (2082 * FP.EN6) * angle * angle), out axis);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
TSVector.Multiply(body.angularVelocity, (FP.Sin(FP.Half * angle * timestep) / angle), out axis);
}
TSQuaternion dorn = new TSQuaternion(axis.x, axis.y, axis.z, FP.Cos(angle * timestep * FP.Half));
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 / (1 + timestep * body.linearDrag);
body.angularVelocity *= 1 / (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);
}
}
}
internal void PostStep()
{
if (_freezeConstraints > 0)
{
bool freezePosX = (_freezeConstraints & TSRigidBodyConstraints.FreezePositionX) == TSRigidBodyConstraints.FreezePositionX;
bool freezePosY = (_freezeConstraints & TSRigidBodyConstraints.FreezePositionY) == TSRigidBodyConstraints.FreezePositionY;
bool freezePosZ = (_freezeConstraints & TSRigidBodyConstraints.FreezePositionZ) == TSRigidBodyConstraints.FreezePositionZ;
if (freezePosX)
{
position.x = _freezePosition.x;
}
if (freezePosY)
{
position.y = _freezePosition.y;
}
if (freezePosZ)
{
position.z = _freezePosition.z;
}
bool freezeRotX = (_freezeConstraints & TSRigidBodyConstraints.FreezeRotationX) == TSRigidBodyConstraints.FreezeRotationX;
bool freezeRotY = (_freezeConstraints & TSRigidBodyConstraints.FreezeRotationY) == TSRigidBodyConstraints.FreezeRotationY;
bool freezeRotZ = (_freezeConstraints & TSRigidBodyConstraints.FreezeRotationZ) == TSRigidBodyConstraints.FreezeRotationZ;
if (freezeRotX || freezeRotY || freezeRotZ)
{
TSQuaternion q = TSQuaternion.CreateFromMatrix(Orientation);
if (freezeRotX)
{
q.x = _freezeRotationQuat.x;
}
if (freezeRotY)
{
q.y = _freezeRotationQuat.y;
}
if (freezeRotZ)
{
q.z = _freezeRotationQuat.z;
}
q.Normalize();
Orientation = TSMatrix.CreateFromQuaternion(q);
}
}
}
public override void PostStep()
{
TSVector pos = Body1.Position;
pos.z = 0;
Body1.Position = pos;
TSQuaternion q = TSQuaternion.CreateFromMatrix(Body1.Orientation);
q.Normalize();
q.x = 0;
q.y = 0;
if (freezeZAxis)
{
q.z = 0;
}
Body1.Orientation = TSMatrix.CreateFromQuaternion(q);
if (Body1.isStatic)
{
return;
}
TSVector vel = Body1.LinearVelocity;
vel.z = 0;
CBFrame.Utils.Logger.Debug("line34 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
Body1.LinearVelocity = vel;
CBFrame.Utils.Logger.Debug("line36 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
TSVector av = Body1.AngularVelocity;
av.x = 0;
av.y = 0;
if (freezeZAxis)
{
av.z = 0;
}
Body1.AngularVelocity = av;
}
public override void PostStep()
{
TSVector pos = Body1.Position;
pos.z = 0;
Body1.Position = pos;
TSQuaternion q = TSQuaternion.CreateFromMatrix(Body1.Orientation);
q.Normalize();
q.x = 0;
q.y = 0;
if (freezeZAxis)
{
q.z = 0;
}
Body1.Orientation = TSMatrix.CreateFromQuaternion(q);
if (Body1.isStatic)
{
return;
}
TSVector vel = Body1.LinearVelocity;
vel.z = 0;
Body1.LinearVelocity = vel;
TSVector av = Body1.AngularVelocity;
av.x = 0;
av.y = 0;
if (freezeZAxis)
{
av.z = 0;
}
Body1.AngularVelocity = av;
}
