/// <summary>
/// Constructs a new constraint which prevents relative angular motion between the two connected bodies.
/// </summary>
/// <param name="connectionA">First connection of the pair.</param>
/// <param name="connectionB">Second connection of the pair.</param>
public NoRotationJoint(Entity connectionA, Entity connectionB)
{
ConnectionA = connectionA;
ConnectionB = connectionB;
initialQuaternionConjugateA = QuaternionEx.Conjugate(ConnectionA.orientation);
initialQuaternionConjugateB = QuaternionEx.Conjugate(ConnectionB.orientation);
}
/// <summary>
/// Computes the quaternion rotation between two normalized vectors.
/// </summary>
/// <param name="v1">First unit-length vector.</param>
/// <param name="v2">Second unit-length vector.</param>
/// <param name="q">System.Numerics.Quaternion representing the rotation from v1 to v2.</param>
public static void GetQuaternionBetweenNormalizedVectors(ref System.Numerics.Vector3 v1, ref System.Numerics.Vector3 v2, out System.Numerics.Quaternion q)
{
float dot;
Vector3Ex.Dot(ref v1, ref v2, out dot);
//For non-normal vectors, the multiplying the axes length squared would be necessary:
//float w = dot + (float)Math.Sqrt(v1.LengthSquared() * v2.LengthSquared());
if (dot < -0.9999f) //parallel, opposing direction
{
//If this occurs, the rotation required is ~180 degrees.
//The problem is that we could choose any perpendicular axis for the rotation. It's not uniquely defined.
//The solution is to pick an arbitrary perpendicular axis.
//Project onto the plane which has the lowest component magnitude.
//On that 2d plane, perform a 90 degree rotation.
float absX = Math.Abs(v1.X);
float absY = Math.Abs(v1.Y);
float absZ = Math.Abs(v1.Z);
if (absX < absY && absX < absZ)
q = new System.Numerics.Quaternion(0, -v1.Z, v1.Y, 0);
else if (absY < absZ)
q = new System.Numerics.Quaternion(-v1.Z, 0, v1.X, 0);
else
q = new System.Numerics.Quaternion(-v1.Y, v1.X, 0, 0);
}
else
{
System.Numerics.Vector3 axis;
Vector3Ex.Cross(ref v1, ref v2, out axis);
q = new System.Numerics.Quaternion(axis.X, axis.Y, axis.Z, dot + 1);
}
q = QuaternionEx.Normalize(q);
}
/// <summary>
/// Constructs a quaternion from a rotation matrix.
/// </summary>
/// <param name="r">Rotation matrix to create the quaternion from.</param>
/// <param name="q">System.Numerics.Quaternion based on the rotation matrix.</param>
public static void CreateFromRotationMatrix(ref Matrix3x3 r, out System.Numerics.Quaternion q)
{
float trace = r.M11 + r.M22 + r.M33;
#if !WINDOWS
q = new System.Numerics.Quaternion();
#endif
if (trace >= 0)
{
var S = (float)Math.Sqrt(trace + 1.0) * 2; // S=4*qw
var inverseS = 1 / S;
q.W = 0.25f * S;
q.X = (r.M23 - r.M32) * inverseS;
q.Y = (r.M31 - r.M13) * inverseS;
q.Z = (r.M12 - r.M21) * inverseS;
}
else if ((r.M11 > r.M22) & (r.M11 > r.M33))
{
var S = (float)Math.Sqrt(1.0 + r.M11 - r.M22 - r.M33) * 2; // S=4*qx
var inverseS = 1 / S;
q.W = (r.M23 - r.M32) * inverseS;
q.X = 0.25f * S;
q.Y = (r.M21 + r.M12) * inverseS;
q.Z = (r.M31 + r.M13) * inverseS;
}
else if (r.M22 > r.M33)
{
var S = (float)Math.Sqrt(1.0 + r.M22 - r.M11 - r.M33) * 2; // S=4*qy
var inverseS = 1 / S;
q.W = (r.M31 - r.M13) * inverseS;
q.X = (r.M21 + r.M12) * inverseS;
q.Y = 0.25f * S;
q.Z = (r.M32 + r.M23) * inverseS;
}
else
{
var S = (float)Math.Sqrt(1.0 + r.M33 - r.M11 - r.M22) * 2; // S=4*qz
var inverseS = 1 / S;
q.W = (r.M12 - r.M21) * inverseS;
q.X = (r.M31 + r.M13) * inverseS;
q.Y = (r.M32 + r.M23) * inverseS;
q.Z = 0.25f * S;
}
}
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>
/// Finds the change in the rotation state quaternion provided the local inertia tensor and angular velocity.
/// </summary>
/// <param name="orientation">Orienatation of the object.</param>
/// <param name="localInertiaTensorInverse">Local-space inertia tensor of the object being updated.</param>
/// <param name="angularMomentum">Angular momentum of the object.</param>
/// <param name="orientationChange">Change in quaternion.</param>
public static void DifferentiateQuaternion(ref System.Numerics.Quaternion orientation, ref Matrix3x3 localInertiaTensorInverse, ref System.Numerics.Vector3 angularMomentum, out System.Numerics.Quaternion orientationChange)
{
System.Numerics.Quaternion normalizedOrientation;
QuaternionEx.Normalize(ref orientation, out normalizedOrientation);
Matrix3x3 tempRotMat;
Matrix3x3.CreateFromQuaternion(ref normalizedOrientation, out tempRotMat);
Matrix3x3 tempInertiaTensorInverse;
Matrix3x3.MultiplyTransposed(ref tempRotMat, ref localInertiaTensorInverse, out tempInertiaTensorInverse);
Matrix3x3.Multiply(ref tempInertiaTensorInverse, ref tempRotMat, out tempInertiaTensorInverse);
System.Numerics.Vector3 halfspin;
Matrix3x3.Transform(ref angularMomentum, ref tempInertiaTensorInverse, out halfspin);
Vector3Ex.Multiply(ref halfspin, .5f, out halfspin);
var halfspinQuaternion = new System.Numerics.Quaternion(halfspin.X, halfspin.Y, halfspin.Z, 0);
QuaternionEx.Multiply(ref halfspinQuaternion, ref normalizedOrientation, out orientationChange);
}
/// <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.
}
/// <summary>
/// Constructs a new bone. Assumes the mass will be set later.
/// </summary>
/// <param name="position">Initial position of the bone.</param>
/// <param name="orientation">Initial orientation of the bone.</param>
/// <param name="radius">Radius of the bone.</param>
/// <param name="height">Height of the bone.</param>
public Bone(System.Numerics.Vector3 position, System.Numerics.Quaternion orientation, float radius, float height)
{
Mass = 1;
Position = position;
Orientation = orientation;
Radius = radius;
Height = height;
}
///<summary>
/// Constructs a new rigid transform.
///</summary>
///<param name="orienation">Rotation component of the transform.</param>
public RigidTransform(System.Numerics.Quaternion orienation)
{
Position = new System.Numerics.Vector3();
Orientation = orienation;
}
///<summary>
/// Constructs a new rigid transform.
///</summary>
///<param name="position">Translation component of the transform.</param>
public RigidTransform(System.Numerics.Vector3 position)
{
Position = position;
Orientation = System.Numerics.Quaternion.Identity;
}
///<summary>
/// Constructs a new rigid transform.
///</summary>
///<param name="position">Translation component of the transform.</param>
///<param name="orientation">Rotation component of the transform.</param>
public RigidTransform(System.Numerics.Vector3 position, System.Numerics.Quaternion orientation)
{
Position = position;
Orientation = orientation;
}
///<summary>
/// Constructs a new entry with identity orientation.
///</summary>
///<param name="shape">Shape of the entry.</param>
public OrientedConvexShapeEntry(ConvexShape shape)
{
Orientation = System.Numerics.Quaternion.Identity;
CollisionShape = shape;
}
///<summary>
/// Constructs a new entry.
///</summary>
///<param name="orientation">Orientation of the entry.</param>
///<param name="shape">Shape of the entry.</param>
public OrientedConvexShapeEntry(System.Numerics.Quaternion orientation, ConvexShape shape)
{
Orientation = orientation;
CollisionShape = shape;
}
