public static void Put(this NetDataWriter writer, Quaternion value)
{
writer.Put(value.X);
writer.Put(value.Y);
writer.Put(value.Z);
writer.Put(value.W);
}
public static void Validate(this System.Numerics.Quaternion q)
{
if (IsInvalid(q.LengthSquared()))
{
throw new NotFiniteNumberException("Invalid value.");
}
}
public static void Write(byte[] bytes, int i, System.Numerics.Quaternion value)
{
Write(bytes, i, value.X);
Write(bytes, i + 4, value.Y);
Write(bytes, i + 8, value.Z);
Write(bytes, i + 12, value.W);
}
/// <summary>
/// Transforms the vector using a quaternion.
/// </summary>
/// <param name="v">Vector to transform.</param>
/// <param name="rotation">Rotation to apply to the vector.</param>
/// <param name="result">Transformed vector.</param>
public static void Transform(ref System.Numerics.Vector3 v, ref System.Numerics.Quaternion rotation, out System.Numerics.Vector3 result)
{
//This operation is an optimized-down version of v' = q * v * q^-1.
//The expanded form would be to treat v as an 'axis only' quaternion
//and perform standard quaternion multiplication. Assuming q is normalized,
//q^-1 can be replaced by a conjugation.
float x2 = rotation.X + rotation.X;
float y2 = rotation.Y + rotation.Y;
float z2 = rotation.Z + rotation.Z;
float xx2 = rotation.X * x2;
float xy2 = rotation.X * y2;
float xz2 = rotation.X * z2;
float yy2 = rotation.Y * y2;
float yz2 = rotation.Y * z2;
float zz2 = rotation.Z * z2;
float wx2 = rotation.W * x2;
float wy2 = rotation.W * y2;
float wz2 = rotation.W * z2;
//Defer the component setting since they're used in computation.
float transformedX = v.X * (1f - yy2 - zz2) + v.Y * (xy2 - wz2) + v.Z * (xz2 + wy2);
float transformedY = v.X * (xy2 + wz2) + v.Y * (1f - xx2 - zz2) + v.Z * (yz2 - wx2);
float transformedZ = v.X * (xz2 - wy2) + v.Y * (yz2 + wx2) + v.Z * (1f - xx2 - yy2);
result.X = transformedX;
result.Y = transformedY;
result.Z = transformedZ;
}
public static NQuaternion GetDeltaQuaternionWithDirectionVectors(NVector3 a, NVector3 b)
{
var dot = NVector3.Dot(a, b);
if (dot < -0.999999)
{
var cross = NVector3.Cross(a, b);
if (cross.Length() < 0.000001)
{
cross = NVector3.Cross(NVector3.UnitY, a);
}
cross = NVector3.Normalize(cross);
return(NQuaternion.CreateFromAxisAngle(cross, Pi));
}
else if (dot > 0.999999)
{
return(new NQuaternion(0, 0, 0, 1));
}
else
{
var xyz = NVector3.Cross(a, b);
var w = (float)(Math.Sqrt(a.Length() * a.Length() + b.Length() * b.Length()) + dot);
return(new NQuaternion(xyz.X, xyz.Y, xyz.Z, w));
}
}
/// <summary>
/// Computes the conjugate of the quaternion.
/// </summary>
/// <param name="quaternion">System.Numerics.Quaternion to conjugate.</param>
/// <param name="result">Conjugated quaternion.</param>
public static void Conjugate(ref System.Numerics.Quaternion quaternion, out System.Numerics.Quaternion result)
{
result.X = -quaternion.X;
result.Y = -quaternion.Y;
result.Z = -quaternion.Z;
result.W = quaternion.W;
}
/// <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 System.Numerics.Matrix4x4 r, out System.Numerics.Quaternion q)
{
Matrix3x3 downsizedMatrix;
Matrix3x3.CreateFromMatrix(ref r, out downsizedMatrix);
CreateFromRotationMatrix(ref downsizedMatrix, out q);
}
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>
/// Converts this System.Numerics Quaternion to a UnityEngine Quaternion, storing values directly in referenced parameter
/// </summary>
public static void ConvertToUnityQuaternion(this System.Numerics.Quaternion source, ref UnityEngine.Quaternion target)
{
target.x = -source.X;
target.y = -source.Y;
target.z = source.Z;
target.w = source.W;
}
/// <summary>
/// Scales a quaternion.
/// </summary>
/// <param name="q">System.Numerics.Quaternion to multiply.</param>
/// <param name="scale">Amount to multiply each component of the quaternion by.</param>
/// <param name="result">Scaled quaternion.</param>
public static void Multiply(ref System.Numerics.Quaternion q, float scale, out System.Numerics.Quaternion result)
{
result.X = q.X * scale;
result.Y = q.Y * scale;
result.Z = q.Z * scale;
result.W = q.W * scale;
}
/// <summary>
/// Computes the axis angle representation of a normalized quaternion.
/// </summary>
/// <param name="q">System.Numerics.Quaternion to be converted.</param>
/// <param name="axis">Axis represented by the quaternion.</param>
/// <param name="angle">Angle around the axis represented by the quaternion.</param>
public static void GetAxisAngleFromQuaternion(ref System.Numerics.Quaternion q, out System.Numerics.Vector3 axis, out float angle)
{
#if !WINDOWS
axis = new System.Numerics.Vector3();
#endif
float qx = q.X;
float qy = q.Y;
float qz = q.Z;
float qw = q.W;
if (qw < 0)
{
qx = -qx;
qy = -qy;
qz = -qz;
qw = -qw;
}
if (qw > 1 - 1e-6)
{
axis = Toolbox.UpVector;
angle = 0;
}
else
{
angle = 2 * (float)Math.Acos(qw);
float denominator = 1 / (float)Math.Sqrt(1 - qw * qw);
axis.X = qx * denominator;
axis.Y = qy * denominator;
axis.Z = qz * denominator;
}
}
/// <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.
}
public virtual Task <bool> SetRotationAsync(Quaternion rotation)
{
async UniTask <bool> RotationTask()
{
if (!ValidationHelper.IsValid(rotation))
{
return(false);
}
await UniTask.SwitchToMainThread();
var unityRotation = rotation.ToUnityQuaternion();
if (m_Rigidbody != null)
{
m_Rigidbody.rotation = unityRotation;
}
else
{
m_Transform.rotation = unityRotation;
}
return(true);
}
return(RotationTask().AsTask());
}
///<summary>
/// Constructs a new compound shape entry using the volume of the shape as a weight.
///</summary>
///<param name="shape">Shape to use.</param>
///<param name="orientation">Local orientation of the shape.</param>
///<param name="weight">Weight of the entry. This defines how much the entry contributes to its owner
/// for the purposes of center of rotation computation.</param>
public CompoundShapeEntry(EntityShape shape, System.Numerics.Quaternion orientation, float weight)
{
orientation.Validate();
LocalTransform = new RigidTransform(orientation);
Shape = shape;
Weight = weight;
}
public static void Write(this IO.EndianWriter s, QuaternionF v)
{
s.Write(v.X);
s.Write(v.Y);
s.Write(v.Z);
s.Write(v.W);
}
public static FLVERBoneTransform FromMatrix4x4(Matrix4x4 m, bool applyMemes)
{
var result = new FLVERBoneTransform();
m.Decompose(out Vector3D s, out Quaternion rq, out Vector3D t);
result.Translation = t.ToNumerics();
if (applyMemes)
{
result.Translation.X = -result.Translation.X;
}
result.Scale = s.ToNumerics();
NMatrix mat = NMatrix.Identity;
if (applyMemes)
{
NQuaternion quat = SapMath.MirrorQuat(rq.ToNumerics());
mat = NMatrix.CreateFromQuaternion(quat);
}
else
{
mat = NMatrix.CreateFromQuaternion(new NQuaternion(rq.X, rq.Y, rq.Z, rq.W));
}
result.Rotation = SapMath.MatrixToEulerXZY(mat);
return(result);
}
/// <summary>
/// Creates a SharpDX rotation matrix from a Numerics Quaternion.
/// </summary>
/// <param name="rotation">The quaternion to use to build the matrix.</param>
public static Matrix RotationQuaternion(Quaternion rotation)
{
Matrix result = Matrix.Identity;
float xx = rotation.X * rotation.X;
float yy = rotation.Y * rotation.Y;
float zz = rotation.Z * rotation.Z;
float xy = rotation.X * rotation.Y;
float zw = rotation.Z * rotation.W;
float zx = rotation.Z * rotation.X;
float yw = rotation.Y * rotation.W;
float yz = rotation.Y * rotation.Z;
float xw = rotation.X * rotation.W;
result.M11 = 1.0f - (2.0f * (yy + zz));
result.M12 = 2.0f * (xy + zw);
result.M13 = 2.0f * (zx - yw);
result.M21 = 2.0f * (xy - zw);
result.M22 = 1.0f - (2.0f * (zz + xx));
result.M23 = 2.0f * (yz + xw);
result.M31 = 2.0f * (zx + yw);
result.M32 = 2.0f * (yz - xw);
result.M33 = 1.0f - (2.0f * (yy + xx));
return(result);
}
/// <summary>
/// Creates a new cylinder cast based wheel shape.
/// </summary>
/// <param name="radius">Radius of the wheel.</param>
/// <param name="width">Width of the wheel.</param>
/// <param name="localWheelOrientation">Unsteered orientation of the wheel in the vehicle's local space.</param>
/// <param name="localGraphicTransform">Local graphic transform of the wheel shape.
/// This transform is applied first when creating the shape's worldTransform.</param>
/// <param name="includeSteeringTransformInCast">Whether or not to include the steering transform in the wheel shape cast. If false, the casted wheel shape will always point straight forward.
/// If true, it will rotate with steering. Sometimes, setting this to false is helpful when the cast shape would otherwise become exposed when steering.</param>
public CylinderCastWheelShape(float radius, float width, System.Numerics.Quaternion localWheelOrientation, System.Numerics.Matrix4x4 localGraphicTransform, bool includeSteeringTransformInCast)
{
shape = new CylinderShape(width, radius);
this.LocalWheelOrientation = localWheelOrientation;
LocalGraphicTransform = localGraphicTransform;
this.IncludeSteeringTransformInCast = includeSteeringTransformInCast;
}
public static string ToString(this NQuaternion quaternion, QuaternionDisplay format)
{
return(format switch
{
QuaternionDisplay.Algebraic => ToAlgebraicString(quaternion),
_ => quaternion.ToString()
});
/// <summary>
/// Creates a rotation matrix from a quaternion.
/// </summary>
/// <param name="quaternion">System.Numerics.Quaternion to convert.</param>
/// <param name="result">Rotation matrix created from the quaternion.</param>
public static void CreateFromQuaternion(ref System.Numerics.Quaternion quaternion, out System.Numerics.Matrix4x4 result)
{
float qX2 = quaternion.X + quaternion.X;
float qY2 = quaternion.Y + quaternion.Y;
float qZ2 = quaternion.Z + quaternion.Z;
float XX = qX2 * quaternion.X;
float YY = qY2 * quaternion.Y;
float ZZ = qZ2 * quaternion.Z;
float XY = qX2 * quaternion.Y;
float XZ = qX2 * quaternion.Z;
float XW = qX2 * quaternion.W;
float YZ = qY2 * quaternion.Z;
float YW = qY2 * quaternion.W;
float ZW = qZ2 * quaternion.W;
result.M11 = 1 - YY - ZZ;
result.M21 = XY - ZW;
result.M31 = XZ + YW;
result.M41 = 0;
result.M12 = XY + ZW;
result.M22 = 1 - XX - ZZ;
result.M32 = YZ - XW;
result.M42 = 0;
result.M13 = XZ - YW;
result.M23 = YZ + XW;
result.M33 = 1 - XX - YY;
result.M43 = 0;
result.M14 = 0;
result.M24 = 0;
result.M34 = 0;
result.M44 = 1;
}
public static void Add(ref System.Numerics.Quaternion a, ref System.Numerics.Quaternion b, out System.Numerics.Quaternion result)
{
result.X = a.X + b.X;
result.Y = a.Y + b.Y;
result.Z = a.Z + b.Z;
result.W = a.W + b.W;
}
//accelerometer tests
//private void updateAccelerometer(IData data) {
// var accel = data.Value<Acceleration>();
// setText(accel.ToString(), 0);
//}
/// <summary>
/// Routine used for collection of Quaternions data of Sensors. In the end, calls a new routine to plot processed Data.
/// </summary>
/// <param name="data"> Quaternion data collected by the sensor</param>
/// <param name="sensorNumber"> number of the sensor used in project, starting by 0 for 1, 1 for 2 and etc </param>
private async Task quaternionStreamRoutine(IData data, int sensorNumber)
{
if (isRunning)
{
var quat = data.Value <Quaternion>();
var eulerAngles = calculateEulerAngles(quat);
var time = data.FormattedTimestamp.ToString();
var year = time.Substring(0, 4); var month = time.Substring(5, 2); var day = time.Substring(8, 2);
var hour = time.Substring(11, 2); var minute = time.Substring(14, 2); var second = time.Substring(17, 2);
var milli = time.Substring(20, 3);
// Store data point
if (record)
{
String newLine = string.Format("{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11}{12}", samples[0], year, month, day, hour, minute, second, milli, quat.W, quat.X, quat.Y, quat.Z, Environment.NewLine);
addPoint(newLine, sensorNumber);
newLine = string.Format("{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11}", samples[0], year, month, day, hour, minute, second, milli, eulerAngles.roll, eulerAngles.pitch, eulerAngles.yaw, Environment.NewLine);
addPointEuler(newLine, sensorNumber);
}
// Update counters
samples[sensorNumber]++;
freq[sensorNumber]++;
// Save reference quaternion
if (shouldCenter[sensorNumber])
{
refEuler[sensorNumber] = eulerAngles;
refQuats[sensorNumber] = quat;
shouldCenter[sensorNumber] = false;
centered[sensorNumber] = true;
}
double angle = 0;
double denom = 1;
if (centered[sensorNumber])
{
WindowsQuaternion a = convertToWindowsQuaternion(refQuats[sensorNumber]);
WindowsQuaternion b = convertToWindowsQuaternion(quat);
quat = centerData(refQuats[sensorNumber], quat);
eulerAngles = centerData(refEuler[sensorNumber], eulerAngles);
angle = (angleMode) ? 2 * Math.Acos(WindowsQuaternion.Dot(a, b) / (a.Length() * b.Length())) * (180 / Math.PI) : 0;
}
else if (angleMode)
{
angle = 2 * Math.Acos(quat.W) * (180 / Math.PI);
denom = Math.Sqrt(1 - Math.Pow(quat.W, 2));
denom = (denom < 0.001) ? 1 : denom; // avoid divide by zero type errors
}
angle = (angle > 180) ? 360 - angle : angle;
await CoreApplication.MainView.CoreWindow.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
plotValues(angle, quat, eulerAngles, denom, sensorNumber);
});
}
}
/// <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>
/// 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 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>
/// Ensures the quaternion has unit length.
/// </summary>
/// <param name="quaternion">System.Numerics.Quaternion to normalize.</param>
/// <param name="toReturn">Normalized quaternion.</param>
public static void Normalize(ref System.Numerics.Quaternion quaternion, out System.Numerics.Quaternion toReturn)
{
float inverse = (float)(1 / Math.Sqrt(quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z + quaternion.W * quaternion.W));
toReturn.X = quaternion.X * inverse;
toReturn.Y = quaternion.Y * inverse;
toReturn.Z = quaternion.Z * inverse;
toReturn.W = quaternion.W * inverse;
}
/// <summary>
/// Create new GameObject
/// </summary>
public GameObject(Vector3 position, Quaternion rotation, Vector3 scale)
{
components = new List <IComponent>();
movement = new Movement(position, rotation, scale);
AddComponent(movement);
active = true;
children = new List <GameObject>();
}
public Task <IVehicle?> SpawnVehicleAsync(Vector3 position, Quaternion rotation, string vehicleAssetId, IVehicleState?state = null)
{
async UniTask <IVehicle?> SpawnVehicleTask()
{
await UniTask.SwitchToMainThread();
if (!ushort.TryParse(vehicleAssetId, out var parsedVehicleId))
{
throw new Exception($"Invalid vehicle id: {vehicleAssetId}");
}
if (Assets.find(EAssetType.VEHICLE, parsedVehicleId) is not VehicleAsset vehicleAsset)
{
return(null);
}
UnturnedVehicle?vehicle = null;
if (state is UnturnedVehicleState && state.StateData?.Length > 0)
{
ReadState(state.StateData, out _ /* id doesn't require i guess? */, out var skinId, out var mythicId,
out var roadPosition, out var fuel, out var health, out var batteryCharge,
out var owner, out var group, out var locked, out byte[][] turrets,
out _, out var tireAliveMask, out var items);
var iVehicle = VehicleManager.SpawnVehicleV3(vehicleAsset, skinId, mythicId, roadPosition,
position.ToUnityVector(), rotation.ToUnityQuaternion(), false, false, false,
false, fuel, health, batteryCharge, owner, group, locked, turrets, tireAliveMask);
if (iVehicle != null)
{
vehicle = new UnturnedVehicle(iVehicle);
if (items != null)
{
foreach (var item in items)
{
iVehicle.trunkItems.loadItem(item.x, item.y, item.rot, item.item);
}
}
}
}
else
{
var iVehicle = VehicleManager.spawnVehicleV2(parsedVehicleId, position.ToUnityVector(),
Quaternion.Identity.ToUnityQuaternion());
if (iVehicle != null)
{
vehicle = new UnturnedVehicle(iVehicle);
}
}
return(vehicle);
}
return(SpawnVehicleTask().AsTask());
}
public static void WriteQuaternion(this JsonWriter writer, System.Numerics.Quaternion q)
{
writer.WriteStartArray();
writer.WriteValue(q.X);
writer.WriteValue(q.Y);
writer.WriteValue(q.Z);
writer.WriteValue(q.W);
writer.WriteEndArray();
}
/// <summary>
/// Computes the inverse of the quaternion.
/// </summary>
/// <param name="quaternion">System.Numerics.Quaternion to invert.</param>
/// <param name="result">Result of the inversion.</param>
public static void Inverse(ref System.Numerics.Quaternion quaternion, out System.Numerics.Quaternion result)
{
float inverseSquaredNorm = quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z + quaternion.W * quaternion.W;
result.X = -quaternion.X * inverseSquaredNorm;
result.Y = -quaternion.Y * inverseSquaredNorm;
result.Z = -quaternion.Z * inverseSquaredNorm;
result.W = quaternion.W * inverseSquaredNorm;
}
public static void Read(this IO.EndianReader s, out QuaternionF v)
{
v = new QuaternionF(
s.ReadSingle(), // I
s.ReadSingle(), // J
s.ReadSingle(), // K
s.ReadSingle() // W
);
}
///<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>
/// 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;
}
}
///<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;
}
///<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;
}
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>
/// 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>
/// 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;
}
