private void PositionChanged(object sender, TrackerChangeEventArgs e)
{
try
{
//TODO: Support user defined sensor index or autodetect
if (e.Sensor == 0)
{
RawPosition = new Vector3D(
-e.Position.X,
-e.Position.Z,
e.Position.Y);
RawRotation = new System.Windows.Media.Media3D.Quaternion(
e.Orientation.Y,
e.Orientation.W,
-e.Orientation.X,
e.Orientation.Z);
UpdatePositionAndRotation();
}
}
catch (Exception exc)
{
Logger.Instance.Error(exc.Message, exc);
}
}
// keepCallbackAlive is to prevent crash from garbage collection not being able to track into the unmanged code of ThreeSpace_API.dll
private void dataCallbackFunc(uint deviceId, IntPtr outputData, uint outputDataLength, IntPtr timestamp)
{
unsafe
{
float *ptr = (float *)outputData;
uint time = ((uint *)timestamp)[0];
Monitor.Enter(this);
this.Dispatcher.Invoke((Action)(() =>
{
/*if (_lastTimeStamp == 0)
* {
* RawPosition = PositionScaleFactor * _positionVec;
* _lastTimeStamp = time;
* }
* else
* {
* double timeDiff = (time - _lastTimeStamp)*0.000001;
* _lastTimeStamp = time;
* _velocityVec += new Vector3D(ptr[4] * timeDiff, ptr[5] * timeDiff, ptr[6] * timeDiff);
* _positionVec += new Vector3D(_velocityVec.X * timeDiff, _velocityVec.Y * timeDiff, _velocityVec.Z * timeDiff);
* RawPosition = _positionVec;
* }*/
RawRotation = new System.Windows.Media.Media3D.Quaternion(
ptr[0],
ptr[1],
ptr[2],
ptr[3]);
UpdatePositionAndRotation();
}));
Monitor.Exit(this);
}
}
protected void trackball_TrackBallMoved(Object sender, EventArgs e)
{
System.Windows.Media.Media3D.Quaternion delta = ((Trackball)sender).Delta;
Vector3 deltaAxis = new Vector3();
deltaAxis.X = (float)delta.Axis.X;
deltaAxis.Y = (float)delta.Axis.Y;
deltaAxis.Z = (float)delta.Axis.Z;
deltaAxis.Normalize();
Vector3 cameraLookDirection = viewport3DImage.CameraTarget - viewport3DImage.CameraPosition;
Vector3 cameraUpDirection = viewport3DImage.CameraUpVector;
cameraLookDirection.Normalize();
// Subtract any component of cameraUpDirection along cameraLookDirection
cameraUpDirection = cameraUpDirection - Vector3.Multiply(cameraLookDirection, Vector3.Dot(cameraUpDirection, cameraLookDirection));
cameraUpDirection.Normalize();
Vector3 cameraX = Vector3.Cross(cameraLookDirection, cameraUpDirection);
// Get axis of rotation in the camera coordinates
Vector3 deltaAxisWorld = Vector3.Multiply(cameraX, deltaAxis.X) +
Vector3.Multiply(cameraUpDirection, deltaAxis.Y) +
Vector3.Multiply(cameraLookDirection, -deltaAxis.Z);
SharpDX.Matrix cameraTransform = SharpDX.Matrix.RotationAxis(deltaAxisWorld, (float)(delta.Angle * Math.PI / 180.0));
viewport3DImage.CameraTarget = Vector3.Transform(viewport3DImage.CameraTarget, cameraTransform).ToVector3();
viewport3DImage.CameraPosition = Vector3.Transform(viewport3DImage.CameraPosition, cameraTransform).ToVector3();
viewport3DImage.CameraUpVector = Vector3.Transform(viewport3DImage.CameraUpVector, cameraTransform).ToVector3();
double newPhi = FindPhi();
if (newPhi != lastPhi)
{
lastPhi = newPhi;
axes.UpdateOpenSides(newPhi);
}
}
public static Quaternion getQuaternion(Vector3D v0, Vector3D v1)
{
Quaternion q = new Quaternion();
// Copy, since cannot modify local
v0.Normalize();
v1.Normalize();
double d = Vector3D.DotProduct(v0, v1);
// If dot == 1, vectors are the same
if (d >= 1.0f)
{
return Quaternion.Identity;
}
double s = Math.Sqrt((1 + d) * 2);
double invs = 1 / s;
Vector3D c = Vector3D.CrossProduct(v0, v1);
q.X = c.X * invs;
q.Y = c.Y * invs;
q.Z = c.Z * invs;
q.W = s * 0.5f;
q.Normalize();
return q;
}
//Returns the quaternion rotation parsed from a Euler angle rotation
public static System.Windows.Media.Media3D.Quaternion GetQuaternionRotation(Vector3D eulerAngles)
{
System.Windows.Media.Media3D.Quaternion rotation = new System.Windows.Media.Media3D.Quaternion(new Vector3D(1, 0, 0), eulerAngles.X);
rotation *= new System.Windows.Media.Media3D.Quaternion(new Vector3D(0, 1, 0), eulerAngles.Y);
rotation *= new System.Windows.Media.Media3D.Quaternion(new Vector3D(0, 0, 1), eulerAngles.Z);
return(rotation);
}
/// <summary>
/// Updates the quadcopter model based on roll pitch and yaw (in radians.)
/// </summary>
/// <param name="roll"></param>
/// <param name="pitch"></param>
/// <param name="yaw"></param>
public void UpdateModel(float roll, float pitch, float yaw)
{
//Credit to http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
// for the math
double heading = -1 * yaw;
//double heading = 0.0;
double attitude = -1 * roll;
double bank = pitch;
double c1 = Math.Cos(heading / 2.0);
double s1 = Math.Sin(heading / 2.0);
double c2 = Math.Cos(attitude / 2.0);
double s2 = Math.Sin(attitude / 2.0);
double c3 = Math.Cos(bank / 2.0);
double s3 = Math.Sin(bank / 2.0);
double c1c2 = c1 * c2;
double s1s2 = s1 * s2;
double w = c1c2 * c3 - s1s2 * s3;
double x = c1c2 * s3 + s1s2 * c3;
double y = s1 * c2 * c3 + c1 * s2 * s3;
double z = c1 * s2 * c3 - s1 * c2 * s3;
Quaternion q = new Quaternion(x, y, z, w);
this.angleRotation.Angle = q.Angle;
this.angleRotation.Axis = q.Axis;
this.Label_Roll.Content = "Roll: " + (roll * 180.0 / Math.PI).ToString("#0.00");
this.Label_Pitch.Content = "Pitch: " + (pitch * 180.0 / Math.PI).ToString("#0.00");
this.Label_Yaw.Content = "Yaw: " + (yaw * 180.0 / Math.PI).ToString("#0.00");
}
private void CalculateCurrentQuaternion(double timeFactor)
{
if (LERPActivated)
{
var x = startQuaternion.X * (1 - timeFactor) + endQuaternion.X * timeFactor;
var y = startQuaternion.Y * (1 - timeFactor) + endQuaternion.Y * timeFactor;
var z = startQuaternion.Z * (1 - timeFactor) + endQuaternion.Z * timeFactor;
var w = startQuaternion.W * (1 - timeFactor) + endQuaternion.W * timeFactor;
currentQuaternion = new Quaternion(x, y, z, w);
currentQuaternion.Normalize();
}
else
{
currentQuaternion = Quaternion.Slerp(startQuaternion, endQuaternion, timeFactor);
double dotProduct = startQuaternion.X * endQuaternion.Y + startQuaternion.Y * endQuaternion.Y
+ startQuaternion.Z * endQuaternion.Z + startQuaternion.W * endQuaternion.W;
var theta = Math.Acos(dotProduct);
if (theta < 0.0) theta = -theta;
var startFactor = Math.Sin((1 - timeFactor) * theta) / Math.Sin(theta);
var endFactor = Math.Sin(timeFactor * theta) / Math.Sin(theta);
var x = startFactor * startQuaternion.X + endFactor * endQuaternion.X;
var y = startFactor * startQuaternion.Y + endFactor * endQuaternion.Y;
var z = startFactor * startQuaternion.Z + endFactor * endQuaternion.Z;
var w = startFactor * startQuaternion.W + endFactor * endQuaternion.W;
currentQuaternion = new Quaternion(x, y, z, w);
currentQuaternion.Normalize();
}
}
//Source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
public static Vector3D QuaternionToEulerAnglesInRad(Quaternion q)
{
q = FormatQuaternion(q);
var sqw = Math.Pow(q.W,2);
var sqx = Math.Pow(q.X,2);
var sqy = Math.Pow(q.Y, 2);
var sqz = Math.Pow(q.Z, 2);
var unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
var test = q.X*q.Y + q.Z*q.W;
if (test > 0.499*unit)
{ // singularity at north pole
return new Vector3D
{
Y = 2*Math.Atan2(q.X, q.W),
Z = Math.PI/2,
X = 0
};
}
if (test < -0.499*unit)
{ // singularity at south pole
return new Vector3D
{
Y = -2*Math.Atan2(q.X, q.W),
Z = -Math.PI/2,
X = 0
};
}
return new Vector3D
{
Y = -Math.Atan2(2*q.Y*q.W - 2*q.X*q.Z, sqx - sqy - sqz + sqw),
Z = Math.Asin(2 * test / unit),
X = -Math.Atan2(2 * q.X * q.W - 2 * q.Y * q.Z, -sqx + sqy - sqz + sqw)
};
}
protected void Track(Point currentPosition)
{
Vector3D currentPosition3D = ProjectToTrackball(
EventSource.ActualWidth, EventSource.ActualHeight, currentPosition);
Vector3D axis = Vector3D.CrossProduct(_previousPosition3D, currentPosition3D);
double angle = Vector3D.AngleBetween(_previousPosition3D, currentPosition3D);
if (angle == 0.0)
{
return;
}
delta = new Quaternion(axis, -angle);
// Get the current orientantion from the RotateTransform3D
AxisAngleRotation3D r = _rotation;
Quaternion q = new Quaternion(_rotation.Axis, _rotation.Angle);
// Compose the delta with the previous orientation
q *= delta;
// Write the new orientation back to the Rotation3D
_rotation.Axis = q.Axis;
_rotation.Angle = q.Angle;
_previousPosition3D = currentPosition3D;
RaiseTrackballMovedEvent(EventArgs.Empty);
}
public static Object.Quaternion CreateNormalized(double x, double y, double z, double w)
{
var quaternion = new Media.Quaternion(x, y, z, w);
quaternion.Normalize();
return(quaternion.Convert());
}
public MobileState(Mobile mob)
{
ObjectInstanceId = mob.ObjectInstanceId;
State = mob.MobileState;
Position = mob.Position;
Rotation = mob.Rotation;
}
public SpatialEntity(string name, string model, Vector3D position, Quaternion rotation)
{
Name = name;
Model = model;
Position = position;
Rotation = rotation;
}
public void ODESolverFunction(double[] y, double x, double[] dy, object obj)
{
var odeSolverData = obj as ODESolverData;
var N = CalculateCubeTorque(new Quaternion(y[3], y[4], y[5], y[6]));
var I = odeSolverData.tensor;
var IInv = odeSolverData.invertTensor;
var W = new Vector3D(y[0], y[1], y[2]);
var IW = MatrixVectorMultiply(I, W);
var IWW = Vector3D.CrossProduct(IW, W);
var NIWW = N + IWW;
var WT = MatrixVectorMultiply(IInv, NIWW);
var Q = new Quaternion(y[3], y[4], y[5], y[6]);
var WQ = new Quaternion(y[0], y[1], y[2], 0);
var QT = Quaternion.Multiply(Q, WQ);
QT = new Quaternion(QT.X / 2, QT.Y / 2, QT.Z / 2, QT.W / 2);
dy[0] = WT.X;
dy[1] = WT.Y;
dy[2] = WT.Z;
dy[3] = QT.X;
dy[4] = QT.Y;
dy[5] = QT.Z;
dy[6] = QT.W;
}
// Updates the matrices of the slaves using the rotation quaternion.
private void UpdateSlaves(Quaternion q, double s, Vector3D t)
{
if (_slaves != null)
{
foreach (Viewport3D i in _slaves)
{
ModelVisual3D mv = i.Children[0] as ModelVisual3D;
Transform3DGroup t3dg = mv.Transform as Transform3DGroup;
ScaleTransform3D _GroupScaleTransform = t3dg.Children[0] as ScaleTransform3D;
RotateTransform3D _GroupRotateTransform = t3dg.Children[1] as RotateTransform3D;
TranslateTransform3D _GroupTranslateTransform = t3dg.Children[2] as TranslateTransform3D;
_GroupScaleTransform.ScaleX = s;
_GroupScaleTransform.ScaleY = s;
_GroupScaleTransform.ScaleZ = s;
_GroupRotateTransform.Rotation = new AxisAngleRotation3D(q.Axis, q.Angle);
_GroupTranslateTransform.OffsetX = t.X;
_GroupTranslateTransform.OffsetY = t.Y;
_GroupTranslateTransform.OffsetZ = t.Z;
}
}
}
public NewtonQuaternion(Quaternion pQuaternion)
{
NWQuaternion[ 0 ] = (float)pQuaternion.X;
NWQuaternion[ 1 ] = (float)pQuaternion.Y;
NWQuaternion[ 2 ] = (float)pQuaternion.Z;
NWQuaternion[ 3 ] = (float)pQuaternion.W;
}
public Matrix3D BuildMatrix3DFromQuaternion(Quaternion q)
{
double sqw = q.W * q.W;
double sqx = q.X * q.X;
double sqy = q.Y * q.Y;
double sqz = q.Z * q.Z;
// invs (inverse square length) is only required if quaternion is not already normalised
double invs = 1 / (sqx + sqy + sqz + sqw);
double m00 = (sqx - sqy - sqz + sqw) * invs; // since sqw + sqx + sqy + sqz =1/invs*invs
double m11 = (-sqx + sqy - sqz + sqw) * invs;
double m22 = (-sqx - sqy + sqz + sqw) * invs;
double tmp1 = q.X * q.Y;
double tmp2 = q.Z * q.W;
double m10 = 2.0 * (tmp1 + tmp2) * invs;
double m01 = 2.0 * (tmp1 - tmp2) * invs;
tmp1 = q.X * q.Z;
tmp2 = q.Y * q.W;
double m20 = 2.0 * (tmp1 - tmp2) * invs;
double m02 = 2.0 * (tmp1 + tmp2) * invs;
tmp1 = q.Y * q.Z;
tmp2 = q.X * q.W;
double m21 = 2.0 * (tmp1 + tmp2) * invs;
double m12 = 2.0 * (tmp1 - tmp2) * invs;
return new Matrix3D(
m00, m01, m02, 0,
m10, m11, m12, 0,
m20, m21, m22, 0,
0, 0, 0, 1
);
}
public MobileState(EntityModel mob)
{
ObjectInstanceId = mob.Id;
State = mob.MobileState;
Position = mob.Position;
Rotation = mob.Rotation;
}
public void SetupInterpolator(double StartAngleR, double StartAngleP, double StartAngleY, double EndAngleR,
double EndAngleP, double EndAngleY, double StartPositionX,
double StartPositionY,
double StartPositionZ,
double EndPositionX,
double EndPositionY,
double EndPositionZ,
Quaternion startQuaternion,
Quaternion endQuaternion
)
{
this.StartAngleP = StartAngleP;
this.StartAngleR = StartAngleR;
this.StartAngleY = StartAngleY;
this.EndAngleR = EndAngleR;
this.EndAngleP = EndAngleP;
this.EndAngleY = EndAngleY;
this.StartPositionX = StartPositionX;
this.StartPositionY = StartPositionY;
this.StartPositionZ = StartPositionZ;
this.EndPositionX = EndPositionX;
this.EndPositionY = EndPositionY;
this.EndPositionZ = EndPositionZ;
this.startQuaternion = startQuaternion;
this.endQuaternion = endQuaternion;
}
public MyPosition(int possessingId, Vector3D position, Quaternion rotation, EnumMobileState mobileState)
{
PossessingId = possessingId;
Position = position;
Rotation = rotation;
MobileState = mobileState;
}
private void Track(Point currentPosition)
{
var currentPosition3D = this.ProjectToTrackball(this.ActualWidth, this.ActualHeight, currentPosition);
var axis = Vector3D.CrossProduct(this._previousPosition3D, currentPosition3D);
var angle = Vector3D.AngleBetween(this._previousPosition3D, currentPosition3D);
// quaterion will throw if this happens - sometimes we can get 3D positions that
// are very similar, so we avoid the throw by doing this check and just ignoring
// the event
if (axis.Length == 0)
return;
var delta = new Quaternion(axis, -angle);
// Get the current orientantion from the RotateTransform3D
var r = this._rotation;
var q = new Quaternion(this._rotation.Axis, this._rotation.Angle);
// Compose the delta with the previous orientation
q *= delta;
// Write the new orientation back to the Rotation3D
this._rotation.Axis = q.Axis;
this._rotation.Angle = q.Angle;
this._previousPosition3D = currentPosition3D;
}
/// <summary>
/// Initializes a new instance of the <see cref="PdbViewState"/> class.
/// </summary>
internal PdbViewState()
{
this.translation = new Vector3D();
this.scale = 1;
this.rotation = Quaternion.Identity;
this.clip = 1;
this.slab = 0;
}
public void Convert(double R, double P, double Y, ref Quaternion startQuaternion)
{
//convert from euler angles to radians
Singleton<MathHelper>.Instance.EulerToRadian(ref R);
Singleton<MathHelper>.Instance.EulerToRadian(ref P);
Singleton<MathHelper>.Instance.EulerToRadian(ref Y);
startQuaternion = ChangeAngles(P, Y, R);
}
public void CalibrateManually(Quaternion quaternion)
{
//CalibrationSystem.Transform = new RotateTransform3D(new QuaternionRotation3D(quaternion));
// Fire event that calibration happened
if (Calibrated != null)
Calibrated(this, new CalibratedEventArgs() { CalibrationQuaternion = quaternion });
}
private Vector3D CalculateCubeTorque(Quaternion cubeQuaternion)
{
if (!GravityEnabled)
return new Vector3D(0, 0, 0);
var G = QuaternionVectorMultiply(cubeQuaternion, gravityVector);
var r = rotationVector;
return Vector3D.CrossProduct(r, G);
}
public void CalculateCurrentQuaternion(ref Quaternion currentQuaternion, double timeFactor)
{
var x = startQuaternion.X * (1 - timeFactor) + endQuaternion.X * timeFactor;
var y = startQuaternion.Y * (1 - timeFactor) + endQuaternion.Y * timeFactor;
var z = startQuaternion.Z * (1 - timeFactor) + endQuaternion.Z * timeFactor;
var w = startQuaternion.W * (1 - timeFactor) + endQuaternion.W * timeFactor;
currentQuaternion = new Quaternion(x, y, z, w);
currentQuaternion.Normalize();
}
private void RootRotationMem_ValueChanged(object sender, object value)
{
Application.Current.Dispatcher.Invoke(() =>
{
System.Windows.Media.Media3D.Quaternion rot = this.RootRotation.ToMedia3DQuaternion();
RotateTransform3D trans = new RotateTransform3D(new QuaternionRotation3D(rot));
this.Root.Transform = trans;
});
}
internal static MilQuaternionF QuaternionToMilQuaternionF(Quaternion q)
{
MilQuaternionF quat;
quat.X = (float) q.X;
quat.Y = (float) q.Y;
quat.Z = (float) q.Z;
quat.W = (float) q.W;
return quat;
}
//Returns the quaternion rotation of a given 3D element
public static System.Windows.Media.Media3D.Quaternion GetQuaternionRotation(Transform3D matrix)
{
Vector3D temp = GetAngleRotation(matrix);
System.Windows.Media.Media3D.Quaternion rotation = new System.Windows.Media.Media3D.Quaternion(new Vector3D(1, 0, 0), temp.X);
rotation *= new System.Windows.Media.Media3D.Quaternion(new Vector3D(0, 1, 0), temp.Y);
rotation *= new System.Windows.Media.Media3D.Quaternion(new Vector3D(0, 0, 1), temp.Z);
return(rotation);
}
public static Vector3D Rotate(Quaternion q, Vector3D v)
{
var conj = new System.Windows.Media.Media3D.Quaternion(q.X, q.Y, q.Z, q.W);
conj.Conjugate();
var rotatedVector = conj * new System.Windows.Media.Media3D.Quaternion(v.X, v.Y, v.Z, 0) * q;
return(new Vector3D(rotatedVector.X, rotatedVector.Y, rotatedVector.Z));
}
public EntityModel(int id, string name, string modelId, Vector3D position, Quaternion rotation)
{
Contract.Requires<ArgumentNullException>(!string.IsNullOrEmpty(name));
Contract.Requires<ArgumentNullException>(!string.IsNullOrEmpty(modelId));
Id = id;
Name = name;
ModelId = modelId;
Position = position;
Rotation = rotation;
}
public static Vector3D GetCornerPointToAxis(CameraPosition cameraPosition, Quaternion axis, Direction direction)
{
// Orientation in Viewing direction
var a1 = cameraPosition.Width / 2;
var a2 = cameraPosition.Height / 2;
var a3 = cameraPosition.FocalLength;
var angleY = Math.Atan(a1 / a3);
var angleX = Math.Atan(a2 / a3);
// Set sign for corner
switch (direction)
{
default:
case Direction.TopLeft:
break;
case Direction.TopRight:
angleX = -angleX;
break;
case Direction.BottomLeft:
angleY = -angleY;
break;
case Direction.BottomRight:
angleX = -angleX;
angleY = -angleY;
break;
}
// Rotation matrices
var xm = new Matrix3D()
{
M11 = 1,
M22 = Math.Cos(angleX),
M32 = Math.Sin(angleX),
M23 = -Math.Sin(angleX),
M33 = Math.Cos(angleX)
};
var ym = new Matrix3D()
{
M11 = Math.Cos(angleY),
M31 = -Math.Sin(angleY),
M22 = 1,
M13 = Math.Sin(angleY),
M33 = Math.Cos(angleY)
};
// Rotate the point by multiplying with quaternion and conjugate of quaternion
var rot = TransformToQuaternion(xm) * TransformToQuaternion(ym) * axis;
return(Rotate(rot, new Vector3D(0, 0, 1)));
}
/// <summary>
/// product of two quaternions
/// </summary>
/// <param name="quaternion1"></param>
/// <param name="quanernion2"></param>
/// <returns></returns>
public static Quaternion ProductOfTwoQuaternion(Quaternion quaternion1, Quaternion quanernion2)
{
Quaternion product = new Quaternion();
//Derived from knowing:
// q1q2 = w1w2 + w1q2 + w2q1 + q1 x q2 - q1.q2
product.X = quaternion1.W * quanernion2.X + quaternion1.X * quanernion2.W + quaternion1.Y * quanernion2.Z - quaternion1.Z * quanernion2.Y;
product.Y = quaternion1.W * quanernion2.Y + quaternion1.Y * quanernion2.W + quaternion1.Z * quanernion2.X - quaternion1.X * quanernion2.Z;
product.Z = quaternion1.W * quanernion2.Z + quaternion1.Z * quanernion2.W + quaternion1.X * quanernion2.Y - quaternion1.Y * quanernion2.X;
product.W = quaternion1.W * quanernion2.W - quaternion1.X * quanernion2.X - quaternion1.Y * quanernion2.Y - quaternion1.Z * quanernion2.Z;
return product;
}
/// <summary>Convertit un <see cref="Object.Quaternion"/> en <see cref="Media.Quaternion"/>.</summary>
public static Media.Quaternion Convert(this Object.Quaternion quaternion)
{
var value = new Media.Quaternion(quaternion.X, quaternion.Y, quaternion.Z, quaternion.W);
if (!value.IsNormalized)
{
value.Normalize();
}
return(value);
}
public static MatrixTransform3D Rotate3D(Transform3D transform, Vector3D look, Vector3D dir, Point3D center)
{
Matrix3D m = new Matrix3D();
Vector3D realook = transform.Transform(look);
Vector3D axis = Vector3D.CrossProduct(realook, dir);
double angle = Math.Acos(Vector3D.DotProduct(realook, dir));
Quaternion q = new Quaternion(axis, angle);
m.RotateAt(q, center);
MatrixTransform3D rlt = transform as MatrixTransform3D;
return new MatrixTransform3D(Matrix3D.Multiply(rlt.Matrix, m));
}
internal static CollisionHull CreateSensorCollisionHull(WorldBase world, Vector3D scale, Quaternion orientation, Point3D position)
{
Transform3DGroup transform = new Transform3DGroup();
//transform.Children.Add(new ScaleTransform3D(scale)); // it ignores scale
transform.Children.Add(new RotateTransform3D(new QuaternionRotation3D(orientation)));
transform.Children.Add(new TranslateTransform3D(position.ToVector()));
// Scale X and Y should be identical, but average them to be safe
double radius = ((SIZEPERCENTOFSCALE_XY * scale.X) + (SIZEPERCENTOFSCALE_XY * scale.Y)) / 2d;
return CollisionHull.CreateCylinder(world, 0, radius, SIZEPERCENTOFSCALE_Z * scale.Z, transform.Value);
}
public void ReadTransform()
{
if (!this.IsEnabled)
{
return;
}
this.Position = this.ViewModel.Position;
this.Rotation = this.ViewModel.Rotation;
this.Scale = this.ViewModel.Scale;
// Convert the character-relative transform into a parent-relative transform
Point3D position = this.Position.ToMedia3DPoint();
Quaternion rotation = this.Rotation.ToMedia3DQuaternion();
if (this.Parent != null)
{
TransformViewModel parentTransform = this.Parent.ViewModel;
Point3D parentPosition = parentTransform.Position.ToMedia3DPoint();
Quaternion parentRot = parentTransform.Rotation.ToMedia3DQuaternion();
parentRot.Invert();
// relative position
position = (Point3D)(position - parentPosition);
// relative rotation
rotation = parentRot * rotation;
// unrotate bones, since we will transform them ourselves.
RotateTransform3D rotTrans = new RotateTransform3D(new QuaternionRotation3D(parentRot));
position = rotTrans.Transform(position);
}
// Store the new parent-relative transform info
this.Position = position.ToCmVector();
this.Rotation = rotation.ToCmQuaternion();
// Set the Media3D hierarchy transforms
this.rotation.Rotation = new QuaternionRotation3D(rotation);
this.position.OffsetX = position.X;
this.position.OffsetY = position.Y;
this.position.OffsetZ = position.Z;
// Draw a line for visualization
if (this.Parent != null && this.lineToParent != null)
{
Point3D p = this.lineToParent.Points[1];
p.X = position.X;
p.Y = position.Y;
p.Z = position.Z;
this.lineToParent.Points[1] = p;
}
}
void MoveUpdateCallback(int id, MoveWrapper.Vector3 pos, MoveWrapper.Quaternion rot, int trigger)
{
RawPosition = new Vector3D(pos.x, pos.y, pos.z);
RawRotation = new Quaternion(rot.x, -rot.y, rot.z, -rot.w);
if (MoveWrapper.getButtonState(0, MoveButton.B_START))
{
Dispatcher.Invoke((Action)(Calibrate));
}
Dispatcher.Invoke((Action)(UpdatePositionAndRotation));
}
public void Transform(Matrix3D transform)
{
var tg = new MatrixTransform3D(transform);
CameraCenter = tg.Transform(CameraCenter.ToPoint3D()).ToVector3D();
var cameraOrientation = new Quaternion(Orientation.X, Orientation.Y, Orientation.Z, Orientation.W);
var left = cameraOrientation;
var right = TransformToQuaternion(transform);
Orientation = left * right;
}
//Copy constructor. Does not copy attachments or ID.
public N8Block(N8Block source, int ID)
{
AttachedTo = null;
Attachees = new List<N8Block>();
this._id = ID;
type = source.type;
name = source.name;
position = source.position;
rotation = source.rotation;
}
protected void UpdateRotation(object sender, EventArgs e)
{
// Event may fire multiple times per render. Don't do unnecessary updates.
TimeSpan nextRenderTime = ((RenderingEventArgs)e).RenderingTime;
if (nextRenderTime != lastRenderTime)
{
lastRenderTime = nextRenderTime;
OpenTK.Quaternion q = rift.PredictedOrientation;
RawRotation = new System.Windows.Media.Media3D.Quaternion(q.X, -q.Y, q.Z, -q.W);
UpdatePositionAndRotation();
}
}
public static Quaternion RotationMatrix(Matrix3D matrix)
{
double sqrt;
double half;
var scale = matrix.M11 + matrix.M22 + matrix.M33;
var result = new Quaternion();
if (scale > 0.0)
{
sqrt = Math.Sqrt(scale + 1.0);
result.W = sqrt * 0.5;
sqrt = 0.5 / sqrt;
result.X = (matrix.M23 - matrix.M32) * sqrt;
result.Y = (matrix.M31 - matrix.M13) * sqrt;
result.Z = (matrix.M12 - matrix.M21) * sqrt;
}
else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))
{
sqrt = Math.Sqrt(1.0 + matrix.M11 - matrix.M22 - matrix.M33);
half = 0.5 / sqrt;
result.X = 0.5 * sqrt;
result.Y = (matrix.M12 + matrix.M21) * half;
result.Z = (matrix.M13 + matrix.M31) * half;
result.W = (matrix.M23 - matrix.M32) * half;
}
else if (matrix.M22 > matrix.M33)
{
sqrt = Math.Sqrt(1.0 + matrix.M22 - matrix.M11 - matrix.M33);
half = 0.5 / sqrt;
result.X = (matrix.M21 + matrix.M12) * half;
result.Y = 0.5 * sqrt;
result.Z = (matrix.M32 + matrix.M23) * half;
result.W = (matrix.M31 - matrix.M13) * half;
}
else
{
sqrt = Math.Sqrt(1.0 + matrix.M33 - matrix.M11 - matrix.M22);
half = 0.5 / sqrt;
result.X = (matrix.M31 + matrix.M13) * half;
result.Y = (matrix.M32 + matrix.M23) * half;
result.Z = 0.5 * sqrt;
result.W = (matrix.M12 - matrix.M21) * half;
}
return(result);
}
public CombatantModel(int id, string name, string modelId, Vector3D position, Quaternion rotation,
float health, float energy, EnumMobileState mobileState, float height,
int constitution, int dexterity, int willpower, int cognition, int strength)
: base(id, name, modelId, position, rotation, health, energy, mobileState, height)
{
Constitution = constitution;
Dexterity = dexterity;
Willpower = willpower;
Cognition = cognition;
Strength = strength;
ActivatingSkill = EnumSkill.None;
SkillQueue = ListModule.Empty<Tuple<EnumSkill, EntityModel>>();
}
protected void UpdateRotation(object sender, EventArgs e)
{
// Event may fire multiple times per render. Don't do unnecessary updates.
TimeSpan nextRenderTime = ((RenderingEventArgs)e).RenderingTime;
if (nextRenderTime != lastRenderTime)
{
lastRenderTime = nextRenderTime;
OpenTK.Quaternion q = rift.PredictedOrientation;
RawRotation = new System.Windows.Media.Media3D.Quaternion(q.X, -q.Y, q.Z, -q.W);
UpdatePositionAndRotation();
}
}
public MotionViewModel(FlightControlConnection connection)
{
// Register handler for the orientation update
connection.RegisterHandler("ROT", rot =>
{
UpdateFrequency = updateFrequencyCounter.Tick();
Rotation = new Quaternion(
double.Parse(rot[0], CultureInfo.InvariantCulture),
double.Parse(rot[1], CultureInfo.InvariantCulture),
double.Parse(rot[2], CultureInfo.InvariantCulture),
double.Parse(rot[3], CultureInfo.InvariantCulture));
;
});
// Register handler for the heading update
connection.RegisterHandler("HEAD", head =>
{
Heading = new Quaternion(
double.Parse(head[0], CultureInfo.InvariantCulture),
double.Parse(head[1], CultureInfo.InvariantCulture),
double.Parse(head[2], CultureInfo.InvariantCulture),
double.Parse(head[3], CultureInfo.InvariantCulture));
});
connection.RegisterHandler("OFFSET", offset =>
{
AngularOffset.Yaw = double.Parse(offset[0], CultureInfo.InvariantCulture);
AngularOffset.Pitch = double.Parse(offset[1], CultureInfo.InvariantCulture);
AngularOffset.Roll = double.Parse(offset[2], CultureInfo.InvariantCulture);
});
_engines = new List<EngineViewModel>
{
new EngineViewModel(5, 10, 10),
new EngineViewModel(6, 10, -10),
new EngineViewModel(9, -10, -10),
new EngineViewModel(10, -10, 10)
};
var enginesByPin = _engines.ToDictionary(e => e.Pin);
connection.RegisterHandler("ENG", eng =>
{
var pin = int.Parse(eng[0]);
enginesByPin[pin].Power = int.Parse(eng[1]);
});
ResetCamera = new RelayCommand(() => CameraRotation = Rotation); // * new Quaternion(0, 0, 1, 0));
AngularOffset = new EulerAngles();
}
/// <summary>
/// 创建纸张快照, folding 用
/// </summary>
/// <param name="vp"></param>
/// <param name="topFaces"></param>
/// <param name="bgFaces"></param>
/// <param name="topImgFront"></param>
/// <param name="bgImg"></param>
public void CreateShadow(Viewport3D vp, List<Face>topFaces, List<Face>bgFaces, Image topImgFront, Image bgImg, Image topImgBack)
{
RenderController render = RenderController.GetInstance();
if (topFaces != null && topFaces.Count != 0)
{
//// 上层纸张
Model3DGroup topModelGroup = new Model3DGroup();
foreach (Face face in topFaces)
{
topModelGroup.Children.Add(render.FaceMeshMap[face]);
}
render.Entity.Content = topModelGroup;
// 正面拍一张照
RenderTargetBitmap bmpf = new RenderTargetBitmap((int)vp.ActualWidth, (int)vp.ActualHeight, 96, 96, PixelFormats.Pbgra32);
bmpf.Render(vp);
topImgFront.Source = bmpf;
// 背面再拍一张照
RenderTargetBitmap bmpb = new RenderTargetBitmap((int)vp.ActualWidth, (int)vp.ActualHeight, 96, 96, PixelFormats.Pbgra32);
Quaternion quat = new Quaternion(0, 1, 0, 0);
quat = quat * render.SrcQuaternion;
render.TransformGroup.Children[0] = new RotateTransform3D(new QuaternionRotation3D(quat));
bmpb.Render(vp);
render.TransformGroup.Children[0] = new RotateTransform3D(new QuaternionRotation3D(render.SrcQuaternion));
topImgBack.Source = bmpb;
// 将topImgBack移到看不见
topImgBack.RenderTransform = tg;
(tg.Children[1] as TranslateTransform).X = -10000;
(tg.Children[1] as TranslateTransform).Y = -10000;
}
if (bgFaces != null && bgFaces.Count != 0)
// 背景纸张
{
Model3DGroup bgModelGroup = new Model3DGroup();
foreach (Face face in bgFaces)
{
bgModelGroup.Children.Add(render.FaceMeshMap[face]);
}
render.Entity.Content = bgModelGroup;
// 正面拍一张照
RenderTargetBitmap bmpbg = new RenderTargetBitmap((int)vp.ActualWidth, (int)vp.ActualHeight, 96, 96, PixelFormats.Pbgra32);
bmpbg.Render(vp);
bgImg.Source = bmpbg;
}
render.Entity.Content = null;
}
public void Rotate(System.Windows.Media.Media3D.Vector3D axis, double angle)
{
var m = viewport3D.Children[viewport3D.Children.Count - 1].Transform as Transform3DGroup;
var _rotation = (m.Children[2] as RotateTransform3D).Rotation as AxisAngleRotation3D;
var q = new System.Windows.Media.Media3D.Quaternion(_rotation.Axis, _rotation.Angle);
var delta = new System.Windows.Media.Media3D.Quaternion(axis, angle);
q *= delta;
_rotation.Axis = q.Axis;
_rotation.Angle = q.Angle;
m.Children[2] = new RotateTransform3D(_rotation);
}
public static Transform3D LibTransformToWPFTransform(Vector3 position, AegirLib.MathType.Quaternion rotation, bool freeze = false)
{
MatrixTransform3D matrixTransform = new MatrixTransform3D();
Matrix3D matrix = new Matrix3D();
System.Windows.Media.Media3D.Quaternion q = new System.Windows.Media.Media3D.Quaternion(rotation.X, rotation.Y, rotation.Z, rotation.W);
matrix.Rotate(q);
matrix.Translate(new Vector3D(position.X, position.Y, position.Z));
matrixTransform.Matrix = matrix;
if (freeze)
{
matrixTransform.Freeze();
}
return(matrixTransform);
}
public static Quaternion RotationAxis(Vector3D axis, double angle)
{
axis.Normalize();
var half = angle * 0.5;
var sin = Math.Sin(half);
var cos = Math.Cos(half);
Quaternion result = new Quaternion
{
X = axis.X * sin,
Y = axis.Y * sin,
Z = axis.Z * sin,
W = cos
};
return(result);
}
public void UpdateViewMatrix()
{
_viewMatrix.SetIdentity();
// fix f*****g crazy z-up
Quaternion zupfix_x = new Quaternion(new Vector3D(1, 0, 0), -90);
Quaternion zupfix_y = new Quaternion(new Vector3D(0, 1, 0), 180);
_viewMatrix.Rotate(zupfix_x);
_viewMatrix.Rotate(zupfix_y);
Quaternion quat1 = new Quaternion(new Vector3D(1, 0, 0), CameraPitch);
Quaternion quat2 = new Quaternion(new Vector3D(0, 1, 0), CameraYaw);
_viewMatrix.Rotate(quat2);
_viewMatrix.Rotate(quat1);
_viewMatrix.Translate(new Vector3D(0, 0, -CameraDistance));
UpdateCamera();
}
public void WriteTransform(ModelVisual3D root, bool writeChildren = true)
{
if (!this.IsEnabled)
{
return;
}
// Apply the current values to the visual tree
this.rotation.Rotation = new QuaternionRotation3D(this.Rotation.ToMedia3DQuaternion());
this.position.OffsetX = this.Position.X;
this.position.OffsetY = this.Position.Y;
this.position.OffsetZ = this.Position.Z;
// convert the values in the tree to character-relative space
MatrixTransform3D transform = (MatrixTransform3D)this.TransformToAncestor(root);
Quaternion rotation = transform.Matrix.ToQuaternion();
rotation.Invert();
CmVector position = default;
position.X = (float)transform.Matrix.OffsetX;
position.Y = (float)transform.Matrix.OffsetY;
position.Z = (float)transform.Matrix.OffsetZ;
// and push those values to the game memory
this.ViewModel.Position = position;
this.ViewModel.Scale = this.Scale;
this.ViewModel.Rotation = rotation.ToCmQuaternion();
if (writeChildren)
{
foreach (Visual3D child in this.Children)
{
if (child is BoneVisual3d childBone)
{
childBone.WriteTransform(root);
}
}
}
}
//Returns the Vector3D transformed by the Quaternion
public static Vector3D Transform(System.Windows.Media.Media3D.Quaternion qt, Vector3D vector)
{
double x2 = qt.X + qt.X;
double y2 = qt.Y + qt.Y;
double z2 = qt.Z + qt.Z;
double wx2 = qt.W * x2;
double wy2 = qt.W * y2;
double wz2 = qt.W * z2;
double xx2 = qt.X * x2;
double xy2 = qt.X * y2;
double xz2 = qt.X * z2;
double yy2 = qt.Y * y2;
double yz2 = qt.Y * z2;
double zz2 = qt.Z * z2;
double x = vector.X * (1.0 - yy2 - zz2) + vector.Y * (xy2 - wz2) + vector.Z * (xz2 + wy2);
double y = vector.X * (xy2 + wz2) + vector.Y * (1.0 - xx2 - zz2) + vector.Z * (yz2 - wx2);
double z = vector.X * (xz2 - wy2) + vector.Y * (yz2 + wx2) + vector.Z * (1.0 - xx2 - yy2);
return(new Vector3D(x, y, z));
}
private void WatchCamera()
{
IInjectionService injection = Services.Get <IInjectionService>();
IMemory <float> camX = injection.GetMemory(Offsets.Main.CameraOffset, Offsets.Main.CameraAngleX);
IMemory <float> camY = injection.GetMemory(Offsets.Main.CameraOffset, Offsets.Main.CameraAngleY);
IMemory <float> camZ = injection.GetMemory(Offsets.Main.CameraOffset, Offsets.Main.CameraRotation);
IMemory <float> camDist = injection.GetMemory(Offsets.Main.CameraOffset, Offsets.Main.CameraCurrentZoom);
Vector3D camEuler = default;
bool vis = true;
while (vis && Application.Current != null)
{
camEuler.Y = (float)MathUtils.RadiansToDegrees((double)camX.Value) - 180;
camEuler.Z = (float)-MathUtils.RadiansToDegrees((double)camY.Value);
camEuler.X = (float)MathUtils.RadiansToDegrees((double)camZ.Value);
Quaternion q = camEuler.ToQuaternion();
try
{
Application.Current.Dispatcher.Invoke(() =>
{
vis = this.IsVisible; ////&& this.IsEnabled;
Transform3DGroup g = new Transform3DGroup();
g.Children.Add(new TranslateTransform3D(0, 0.75, -(camDist.Value - 1)));
g.Children.Add(new RotateTransform3D(new QuaternionRotation3D(q)));
this.Viewport.Camera.Transform = g;
});
}
catch (Exception)
{
}
Thread.Sleep(16);
}
}
private void OnMavlinkMessageReceived(object sender, MavLinkMessage e)
{
if (currentFlightLog != null && !pauseRecording)
{
currentFlightLog.AddMessage(e);
}
switch (e.MsgId)
{
case MAVLink.MAVLINK_MSG_ID.ATTITUDE_QUATERNION:
{
var payload = (MAVLink.mavlink_attitude_quaternion_t)e.TypedPayload;
var q = new System.Windows.Media.Media3D.Quaternion(payload.q1, payload.q2, payload.q3, payload.q4);
UiDispatcher.RunOnUIThread(() =>
{
//ModelViewer.ModelAttitude = initialAttitude * q;
});
break;
}
case MAVLink.MAVLINK_MSG_ID.ATTITUDE:
{
var payload = (MAVLink.mavlink_attitude_t)e.TypedPayload;
Quaternion y = new Quaternion(new Vector3D(0, 0, 1), -payload.yaw * 180 / Math.PI);
Quaternion x = new Quaternion(new Vector3D(1, 0, 0), payload.pitch * 180 / Math.PI);
Quaternion z = new Quaternion(new Vector3D(0, 1, 0), payload.roll * 180 / Math.PI);
UiDispatcher.RunOnUIThread(() =>
{
ModelViewer.ModelAttitude = initialAttitude * (y * x * z);
});
break;
}
case MAVLink.MAVLINK_MSG_ID.HIL_STATE_QUATERNION:
{
var payload = (MAVLink.mavlink_hil_state_quaternion_t)e.TypedPayload;
Quaternion q = new Quaternion(payload.attitude_quaternion[0],
payload.attitude_quaternion[1],
payload.attitude_quaternion[2],
payload.attitude_quaternion[3]);
UiDispatcher.RunOnUIThread(() =>
{
ModelViewer.ModelAttitude = initialAttitude * q;
});
break;
}
case MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT:
{
var payload = (MAVLink.mavlink_global_position_int_t)e.TypedPayload;
UiDispatcher.RunOnUIThread(() =>
{
MapLocation((double)payload.lat / 1e7, (double)payload.lon / 1e7);
});
break;
}
case MAVLink.MAVLINK_MSG_ID.SERIAL_CONTROL:
{
var ctrl = (MAVLink.mavlink_serial_control_t)e.TypedPayload;
if (ctrl.count > 0)
{
string text = System.Text.Encoding.ASCII.GetString(ctrl.data, 0, ctrl.count);
UiDispatcher.RunOnUIThread(() =>
{
SystemConsole.Write(text);
});
}
break;
}
case MAVLink.MAVLINK_MSG_ID.DATA_TRANSMISSION_HANDSHAKE:
if (showImageStream)
{
var p = (MAVLink.mavlink_data_transmission_handshake_t)e.TypedPayload;
incoming_image.size = p.size;
incoming_image.packets = p.packets;
incoming_image.payload = p.payload;
incoming_image.quality = p.jpg_quality;
incoming_image.type = p.type;
incoming_image.width = p.width;
incoming_image.height = p.height;
incoming_image.start = Environment.TickCount;
incoming_image.packetsArrived = 0;
incoming_image.data = new byte[incoming_image.size];
}
break;
case MAVLink.MAVLINK_MSG_ID.ENCAPSULATED_DATA:
if (showImageStream)
{
var img = (MAVLink.mavlink_encapsulated_data_t)e.TypedPayload;
int seq = img.seqnr;
uint pos = (uint)seq * (uint)incoming_image.payload;
// Check if we have a valid transaction
if (incoming_image.packets == 0 || incoming_image.size == 0)
{
// not expecting an image?
incoming_image.packetsArrived = 0;
break;
}
uint available = (uint)incoming_image.payload;
if (pos + available > incoming_image.size)
{
available = incoming_image.size - pos;
}
Array.Copy(img.data, 0, incoming_image.data, pos, available);
progress.ShowProgress(0, incoming_image.size, pos + available);
++incoming_image.packetsArrived;
//Debug.WriteLine("packet {0} of {1}, position {2} of {3}", incoming_image.packetsArrived, incoming_image.packets,
// pos + available, incoming_image.size);
// emit signal if all packets arrived
if (pos + available >= incoming_image.size)
{
// Restart state machine
incoming_image.packets = 0;
incoming_image.packetsArrived = 0;
byte[] saved = incoming_image.data;
incoming_image.data = null;
UiDispatcher.RunOnUIThread(() =>
{
progress.ShowProgress(0, 0, 0);
ShowImage(saved);
});
}
}
break;
}
}
/// <summary>
/// Implementation of the Magdewick algorithm, using IMU data to update orientation Quaternion.
/// </summary>
/// <param name="gx">Gyro X value</param>
/// <param name="gy">Gyro Y value</param>
/// <param name="gz">Gyro Z value</param>
/// <param name="ax">Accel X value</param>
/// <param name="ay">Accel Y value</param>
/// <param name="az">Accel Z value</param>
/// <param name="mx">Mag X value</param>
/// <param name="my">Mag Y value</param>
/// <param name="mz">Mag Z value</param>
public void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
float q1 = (float)processed.W, q2 = (float)processed.X, q3 = (float)processed.Y, q4 = (float)processed.Z; // short name local variable for readability
float norm;
float hx, hy, _2bx, _2bz, _8bx, _8bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
// Auxiliary variables to avoid repeated arithmetic
float _2q1mx;
float _2q1my;
float _2q1mz;
float _2q2mx;
float _4bx;
float _4bz;
float _2q1 = 2f * q1;
float _2q2 = 2f * q2;
float _2q3 = 2f * q3;
float _2q4 = 2f * q4;
float _2q1q3 = 2f * q1 * q3;
float _2q3q4 = 2f * q3 * q4;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = (float)Math.Sqrt(ax * ax + ay * ay + az * az);
if (norm == 0f)
{
return; // handle NaN
}
norm = 1 / norm; // use reciprocal for division
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = (float)Math.Sqrt(mx * mx + my * my + mz * mz);
if (norm == 0f)
{
return; // handle NaN
}
norm = 1 / norm; // use reciprocal for division
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
_2q1mx = 2f * q1 * mx;
_2q1my = 2f * q1 * my;
_2q1mz = 2f * q1 * mz;
_2q2mx = 2f * q2 * mx;
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
_2bx = (float)Math.Sqrt(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
_4bx = 2f * _2bx;
_4bz = 2f * _2bz;
_8bx = 2f * _4bx;
_8bz = 2f * _4bz;
// Gradient decent algorithm corrective step
s1 = -_2q3 * (2f * q2q4 - _2q1q3 - ax) + _2q2 * (2f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s2 = _2q4 * (2f * q2q4 - _2q1q3 - ax) + _2q1 * (2f * q1q2 + _2q3q4 - ay) - 4f * q2 * (1 - 2f * q2q2 - 2f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s3 = -_2q1 * (2f * q2q4 - _2q1q3 - ax) + _2q4 * (2f * q1q2 + _2q3q4 - ay) - 4f * q3 * (1 - 2f * q2q2 - 2f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s4 = _2q2 * (2f * q2q4 - _2q1q3 - ax) + _2q3 * (2f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
norm = 1f / (float)Math.Sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;
// Compute rate of change of quaternion
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - 0.5f * s1;
qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - 0.5f * s2;
qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - 0.5f * s3;
qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - 0.5f * s4;
// Integrate to yield quaternion
q1 += qDot1 * step_size;
q2 += qDot2 * step_size;
q3 += qDot3 * step_size;
q4 += qDot4 * step_size;
norm = 1f / (float)Math.Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
processed = new Quaternion(q2 * norm, q3 * norm, q4 * norm, q1 * norm);
}
/// <summary>Convertit un <see cref="Media.Quaternion"/> en <see cref="Object.Quaternion"/>.</summary> public static Object.Quaternion Convert(this Media.Quaternion quaternion) => new Object.Quaternion(quaternion.X, quaternion.Y, quaternion.Z, quaternion.W);
//Returns the Vector3D result of the rotation
public static Vector3D Rotate(Vector3D vector, Vector3D axis, double angle)
{
System.Windows.Media.Media3D.Quaternion qt = new System.Windows.Media.Media3D.Quaternion(axis, angle);
return(Transform(qt, vector));
}
