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> /// 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> /// 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; }