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;
}
}
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);
}
}
}
}
internal static Double GetSegmentLengthQuaternion(Quaternion from, Quaternion to)
{
from.Invert();
return (to * from).Angle;
}
internal static Quaternion SubtractQuaternion(Quaternion value1, Quaternion value2)
{
value2.Invert();
return value1 * value2;
}
public void Update3dPhoneModel(SensorRawFeatureSet reading)
{
var quat = new Quaternion(reading.QuaternionX, reading.QuaternionY,
reading.QuaternionZ, reading.QuaternionW);
GlobalSystem.Transform = new RotateTransform3D(new QuaternionRotation3D(quat));
if (calibrate)
{
quat.Invert();
CalibrateManually(quat);
calibrate = false;
}
}