/// <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.
Vector3.Add(ref Position, ref linearVelocity, out Position);
//Update the orientation based on the angular velocity.
Vector3 increment;
Vector3.Multiply(ref angularVelocity, .5f, out increment);
var multiplier = new Quaternion(increment.X, increment.Y, increment.Z, 0);
Quaternion.Multiply(ref multiplier, ref Orientation, out multiplier);
Quaternion.Add(ref Orientation, ref multiplier, out Orientation);
Orientation.Normalize();
//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 Vector3();
angularVelocity = new 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.
}
void IPositionUpdateable.PreUpdatePosition(float dt)
{
Vector3f increment;
Vector3f.Multiply(ref angularVelocity, dt * .5f, out increment);
var multiplier = new Quaternion(0, increment.X, increment.Y, increment.Z);
Quaternion.Multiply(ref multiplier, ref orientation, out multiplier);
Quaternion.Add(ref orientation, ref multiplier, out orientation);
orientation.Normalize();
Matrix3f.FromQuaternion(ref orientation, out orientationMatrix);
//Only do the linear motion if this object doesn't obey CCD.
if (PositionUpdateMode == PositionUpdateMode.Discrete)
{
Vector3f.Multiply(ref linearVelocity, dt, out increment);
Vector3f.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
}
void IPositionUpdateable.PreUpdatePosition(float dt)
{
Vector3 increment;
if (MotionSettings.UseRk4AngularIntegration && isDynamic)
{
Toolbox.UpdateOrientationRK4(ref orientation, ref localInertiaTensorInverse, ref angularMomentum, dt, out orientation);
}
else
{
Vector3.Multiply(ref angularVelocity, dt * .5f, out increment);
var multiplier = new Quaternion(increment.X, increment.Y, increment.Z, 0);
Quaternion.Multiply(ref multiplier, ref orientation, out multiplier);
Quaternion.Add(ref orientation, ref multiplier, out orientation);
orientation.Normalize();
}
Matrix3X3.CreateFromQuaternion(ref orientation, out orientationMatrix);
//Only do the linear motion if this object doesn't obey CCD.
if (PositionUpdateMode == PositionUpdateMode.Discrete)
{
Vector3.Multiply(ref linearVelocity, dt, out increment);
Vector3.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);
}
}
collisionInformation.UpdateWorldTransform(ref position, ref orientation);
}
public void Addition()
{
Quaternion a = new Quaternion(1.0f, 2.0f, 3.0f, 4.0f);
Quaternion b = new Quaternion(2.0f, 3.0f, 4.0f, 5.0f);
Quaternion c = Quaternion.Add(a, b);
Assert.AreEqual(new Quaternion(3.0f, 5.0f, 7.0f, 9.0f), c);
}
public void Quaternion_AddsCorrectly()
{
var quaternion1 = new Quaternion(25, 20, 10, 30);
var quaternion2 = new Quaternion(100, 50, 70, 80);
var result = Quaternion.Add(quaternion1, quaternion2);
TheResultingValue(result).WithinDelta(0.1f)
.ShouldBe(125.0f, 70.0f, 80.0f, 110.0f);
}
public void QuaternionAddTest()
{
Quaternion a = new Quaternion(1.0f, 2.0f, 3.0f, 4.0f);
Quaternion b = new Quaternion(5.0f, 6.0f, 7.0f, 8.0f);
Quaternion expected = new Quaternion(6.0f, 8.0f, 10.0f, 12.0f);
Quaternion actual;
actual = Quaternion.Add(a, b);
Assert.AreEqual(expected, actual, "Quaternion.Add did not return the expected value.");
}
public void SubtractAdd()
{
Quaternion q = Quaternion.CreateFromYawPitchRoll(0.4, 0.6, 0.1);
Quaternion p = Quaternion.CreateFromYawPitchRoll(0.8, 0.2, 0.3);
Quaternion recovered = p.Add(q.Subtract(p));
Assert.AreEqual(q.W, recovered.W, 1e-3);
Assert.AreEqual(q.X, recovered.X, 1e-3);
Assert.AreEqual(q.Y, recovered.Y, 1e-3);
Assert.AreEqual(q.Z, recovered.Z, 1e-3);
}
public void QuaternionAddTest()
{
Quaternion a = new Quaternion(1.0f, 2.0f, 3.0f, 4.0f);
Quaternion b = new Quaternion(5.0f, 6.0f, 7.0f, 8.0f);
Quaternion expected = new Quaternion(6.0f, 8.0f, 10.0f, 12.0f);
Quaternion actual;
actual = Quaternion.Add(a, b);
Assert.Equal(expected, actual);
}
public void QuaternionAddTest()
{
Quaternion <float> a = new Quaternion <float>(1.0f, 2.0f, 3.0f, 4.0f);
Quaternion <float> b = new Quaternion <float>(5.0f, 6.0f, 7.0f, 8.0f);
Quaternion <float> expected = new Quaternion <float>(6.0f, 8.0f, 10.0f, 12.0f);
Quaternion <float> actual;
actual = Quaternion <float> .Add(a, b);
Assert.Equal(expected, actual);
}
public void AddSubtract()
{
Quaternion q = Quaternion.CreateFromYawPitchRoll(0.4, 0.6, 0.1);
double[] lie = new double[3] {
0.5, 0.2, 0.3
};
double[] recovered = q.Add(lie).Subtract(q);
Assert.AreEqual(lie[0], recovered[0], 1e-3);
Assert.AreEqual(lie[1], recovered[1], 1e-3);
Assert.AreEqual(lie[2], recovered[2], 1e-3);
}
public void Add()
{
Quaternion q1 = new Quaternion(1, 2, 3, 4);
Quaternion q2 = new Quaternion(1, 2, 3, 4);
Quaternion expected = new Quaternion(2, 4, 6, 8);
Compare(expected, Quaternion.Add(q1, q2));
Quaternion result;
Quaternion.Add(ref q1, ref q2, out result);
Compare(expected, result);
}
public void AddTest()
{
Quaternion left = Utilities.GenerateQuaternion();
Quaternion right = Utilities.GenerateQuaternion();
Quaternion expected = Utilities.ConvertFrom(Microsoft.Xna.Framework.Quaternion.Add(
Utilities.ConvertToXna(left),
Utilities.ConvertToXna(right)));
Quaternion actual;
actual = Quaternion.Add(left, right);
Utilities.AreEqual(expected, actual);
}
public void Add()
{
var q1 = new Quaternion();
q1.Set(1, 2, 3, 4);
var q2 = q1.Added(new Quaternion(2, 3, 4, 5));
Assert.AreEqual(3, q2.X, 0.000001);
Assert.AreEqual(5, q2.Y, 0.000001);
Assert.AreEqual(7, q2.Z, 0.000001);
Assert.AreEqual(9, q2.W, 0.000001);
q1.Add(new Quaternion(2, 3, 4, 5));
Assert.AreEqual(3, q1.X, 0.000001);
Assert.AreEqual(5, q1.Y, 0.000001);
Assert.AreEqual(7, q1.Z, 0.000001);
Assert.AreEqual(9, q1.W, 0.000001);
}
public void AddByRefTest()
{
Quaternion left = Utilities.GenerateQuaternion();
Quaternion leftExpected = left;
Quaternion right = Utilities.GenerateQuaternion();
Quaternion rightExpected = right;
Quaternion result;
Quaternion resultExpected = Utilities.ConvertFrom(Microsoft.Xna.Framework.Quaternion.Add(
Utilities.ConvertToXna(left),
Utilities.ConvertToXna(right)));
Quaternion.Add(ref left, ref right, out result);
Utilities.AreEqual(leftExpected, left);
Utilities.AreEqual(rightExpected, right);
Utilities.AreEqual(resultExpected, result);
}
// Tries to compute sensible tangent values for the quaternion
static Quaternion Intermediate(Quaternion q1, Quaternion q2, Quaternion q3)
{
Quaternion q2inv = Quaternion.Inverse(q2);
Quaternion c1 = q2inv * q3;
Quaternion c2 = q2inv * q1;
c1 = c1.Loged();
c2 = c2.Loged();
Quaternion c3 = c1.Add(c2); //bytte ordning på c1 å c2
c3 = c3.Scaled(-0.25f);
c3 = c3.Exped();
Quaternion r_quat = q2 * c3;
r_quat = r_quat.Normalized();
return(r_quat);
}
public void TestAdd()
{
Quaternion a = new Quaternion(1, 2, 3, 4);
Quaternion b = new Quaternion(10, 20, 30, 40);
Quaternion s = a + b;
Quaternion t = Quaternion.Add(a, b);
Quaternion.Add(out var u, ref a, ref b);
a.Add(b); // mutates a
Quaternion r = new Quaternion(11, 22, 33, 44);
Assert.AreEqual(r, s);
Assert.AreEqual(r, t);
Assert.AreEqual(r, u);
Assert.AreEqual(r, a);
}
public override void ExclusiveUpdate()
{
if (Plane.DriverSeat.Sitter == null)
{
return; // Don't fly when there's nobody driving this!
}
// TODO: Special case for motion on land: only push forward if W key is pressed? Or maybe apply that rule in general?
// Collect the plane's relative vectors
Vector3 forward = Quaternion.Transform(Vector3.UnitY, Entity.Orientation);
Vector3 side = Quaternion.Transform(Vector3.UnitX, Entity.Orientation);
Vector3 up = Quaternion.Transform(Vector3.UnitZ, Entity.Orientation);
// Engines!
if (Plane.FastOrSlow >= 0.0)
{
Vector3 force = forward * (Plane.RegularStrength + Plane.FastStrength) * Delta;
entity.ApplyLinearImpulse(ref force);
}
double dotforw = Vector3.Dot(entity.LinearVelocity, forward);
entity.ApplyImpulse(side * 5 + entity.Position, up * -Plane.RightLeft * entity.Mass * dotforw * 0.5 * Delta);
entity.ApplyImpulse(forward * 5 + entity.Position, side * ((Plane.IRight ? 1 : 0) + (Plane.ILeft ? -1 : 0)) * entity.Mass * dotforw * 0.5 * Delta);
entity.ApplyImpulse(forward * 5 + entity.Position, up * Plane.ForwBack * entity.Mass * 0.5 * Delta * dotforw);
// Rotate the entity pre-emptively, and re-apply the movement velocity in this new direction!
double vellen = entity.LinearVelocity.Length();
Vector3 normvel = entity.LinearVelocity / vellen;
Vector3 norm_vel_transf = Quaternion.Transform(normvel, Quaternion.Inverse(entity.Orientation)); // Probably just 1,0,0 on whichever axis... can be simplified!
Vector3 inc = entity.AngularVelocity * Delta * 0.5;
Quaternion quat = new Quaternion(inc.X, inc.Y, inc.Z, 0);
quat = quat * entity.Orientation;
Quaternion orient = entity.Orientation;
Quaternion.Add(ref orient, ref quat, out orient);
orient.Normalize();
entity.Orientation = orient;
entity.LinearVelocity = Quaternion.Transform(norm_vel_transf, orient) * vellen;
entity.AngularVelocity *= 0.1;
// Apply air drag
Entity.ModifyLinearDamping(Plane.FastOrSlow < 0.0 ? 0.6 : 0.1); // TODO: arbitrary constant
Entity.ModifyAngularDamping(0.5); // TODO: arbitrary constant
// Ensure we're active if flying!
Entity.ActivityInformation.Activate();
}
public void QuaternionOperators()
{
Quaternion a = new Quaternion(0.1, 0.2, 0.3, 0.4);
Quaternion b = new Quaternion(0.4, 0.3, 0.2, 0.1);
Assert.AreEqual(new Quaternion(0.1, 0.2, 0.3, 0.4), a);
Assert.AreEqual(new Quaternion(0.4, 0.3, 0.2, 0.1), b);
Quaternion result1 = a + b;
Quaternion result2 = a - b;
Assert.AreEqual(0.5, result1.W, "VectorOperators Sum Quaternion - W");
Assert.AreEqual(0.5, result1.X, "VectorOperators Sum Quaternion - X");
Assert.AreEqual(0.5, result1.Y, "VectorOperators Sum Quaternion - Y");
Assert.AreEqual(0.5, result1.Z, "VectorOperators Sum Quaternion - Z");
Assert.AreEqual(-0.3, result2.W, tolerance, "VectorOperators Subs Quaternion - W");
Assert.AreEqual(-0.1, result2.X, tolerance, "VectorOperators Subs Quaternion - X");
Assert.AreEqual(0.1, result2.Y, tolerance, "VectorOperators Subs Quaternion - Y");
Assert.AreEqual(0.3, result2.Z, tolerance, "VectorOperators Subs Quaternion - Z");
Assert.AreEqual(result1, a + b);
Assert.AreEqual(new Quaternion(0.5, 0.5, 0.5, 0.5), result1);
Assert.AreEqual(new Quaternion(0.5, 0.5, 0.5, 0.5), a + b);
Assert.AreEqual(result2, a - b);
Assert.AreEqual(new Quaternion(-0.3, -0.1, 0.1, 0.3), result2);
Assert.AreEqual(new Quaternion(-0.3, -0.1, 0.1, 0.3), a - b);
Assert.AreEqual(new Quaternion(0.5, 0.5, 0.5, 0.5), a += b);
Assert.AreEqual(new Quaternion(0.1, 0.2, 0.3, 0.4), a -= b);
a.Add(b);
Assert.AreEqual(new Quaternion(0.5, 0.5, 0.5, 0.5), a);
a.Subtract(b);
Assert.AreEqual(new Quaternion(0.1, 0.2, 0.3, 0.4), a);
}
public Quaternion AddBenchmark() => Quaternion.Add(Quaternion.Identity, Quaternion.Identity);
public static Quaternion operator -(Quaternion q, Quaternion r)
{
return(Quaternion.Add(q, -r));
}
/// <summary>
/// Adds two quaternions.
/// </summary>
/// <param name="m1">Source Quaternion.</param>
/// <param name="m2">Source Quaternion.</param>
/// <returns>Sum of the two source Quaternion.</returns>
public static TGCQuaternion Add(TGCQuaternion m1, TGCQuaternion m2)
{
return(new TGCQuaternion(Quaternion.Add(m1.ToQuaternion(), m2.ToQuaternion())));
}
public override void ProcessFrame(Playable playable, FrameData info, object playerData)
{
Transform targetTransform = playerData as Transform;
if (targetTransform == null)
{
return;
}
Vector3 originalPosition = targetTransform.position;
Quaternion originalRotation = targetTransform.rotation;
Vector3 originalScale = targetTransform.localScale;
Vector3 originalEulerAngles = targetTransform.localEulerAngles;
Vector3 outputPosition = Vector3.zero;
Quaternion outputRotation = new Quaternion(0f, 0f, 0f, 0f);
Vector3 outputScale = Vector3.zero;
Vector3 outputEulerAngles = Vector3.zero;
float positionWeight = 0f;
float rotationWeight = 0f;
float scaleWeight = 0f;
int inputCount = playable.GetInputCount();
for (int i = 0; i < inputCount; i++)
{
ScriptPlayable <EZTransformTweenBehaviour> inputPlayable = (ScriptPlayable <EZTransformTweenBehaviour>)playable.GetInput(i);
EZTransformTweenBehaviour inputBehaviour = inputPlayable.GetBehaviour();
if (inputBehaviour.endPoint == null)
{
continue;
}
float inputWeight = playable.GetInputWeight(i);
if (!started && !inputBehaviour.startPoint)
{
inputBehaviour.startPosition = originalPosition;
inputBehaviour.startRotation = originalRotation;
inputBehaviour.startScale = originalScale;
}
float normalizedTime = (float)(inputPlayable.GetTime() / inputPlayable.GetDuration());
float process = inputBehaviour.curve.Evaluate(normalizedTime);
if (inputBehaviour.tweenPosition)
{
positionWeight += inputWeight;
outputPosition += Vector3.Lerp(inputBehaviour.startPosition, inputBehaviour.endPoint.position, process) * inputWeight;
}
if (inputBehaviour.tweenRotation)
{
rotationWeight += inputWeight;
Quaternion targetRotation = Quaternion.Lerp(inputBehaviour.startRotation, inputBehaviour.endPoint.rotation, process);
#if UNITY_2018_1
targetRotation = targetRotation.Normalize();
#else
targetRotation.Normalize();
#endif
if (Quaternion.Dot(outputRotation, targetRotation) < 0f)
{
targetRotation = targetRotation.Scale(-1f);
}
targetRotation = targetRotation.Scale(inputWeight);
outputRotation = outputRotation.Add(targetRotation);
outputEulerAngles += Vector3.Lerp(inputBehaviour.startEulerAngles, inputBehaviour.endEulerAngles, process) * inputWeight;
}
if (inputBehaviour.tweenScale)
{
scaleWeight += inputWeight;
outputScale += Vector3.Lerp(inputBehaviour.startScale, inputBehaviour.endPoint.localScale, process) * inputWeight;
}
}
targetTransform.position = outputPosition + originalPosition * (1f - positionWeight);
if (eulerRotation)
{
targetTransform.localEulerAngles = outputEulerAngles + originalEulerAngles * (1f - rotationWeight);
}
else
{
targetTransform.rotation = outputRotation.Add(originalRotation.Scale(1f - rotationWeight));
}
targetTransform.localScale = outputScale + originalScale * (1f - scaleWeight);
started = true;
}
public void ValueType_Quaternion()
{
Quaternion q1 = new Quaternion(new Vec(1, 0, 0), Math.PI);
Assert.AreEqual("1,0,0,6.12323399573677E-17", q1.ToString());
q1 = new Quaternion(new Vec(1, 0, 0), new Vec(1, 2, 0));
Assert.AreEqual("0,0,0.525731112119134,0.85065080835204", q1.ToString());
var mat = new Mat();
mat.SetIdentity();
q1 = new Quaternion(mat);
Assert.AreEqual("0,0,0,1", q1.ToString());
Assert.IsTrue(q1.IsEqual(new Quaternion(0, 0, 0, 1)));
Assert.AreEqual(mat, q1.GetMatrix());
q1 = new Quaternion(new Vec(1, 0, 0), Math.PI);
Vec vec = new Vec();
double angle = 0;
q1.GetVectorAndAngle(ref vec, ref angle);
Assert.AreEqual(Math.PI, angle);
Assert.AreEqual(new Vec(1, 0, 0), vec);
q1.SetEulerAngles(EulerSequence.EulerAngles, 1.0, 2.0, 3.0);
double a = 0;
double b = 0;
double c = 0;
q1.GetEulerAngles(EulerSequence.EulerAngles, ref a, ref b, ref c);
Assert.AreEqual(1.0, a, 0.0000001);
Assert.AreEqual(2.0, b, 0.0000001);
Assert.AreEqual(3.0, c, 0.0000001);
q1.SetIdent();
Assert.AreEqual("0,0,0,1", q1.ToString());
q1.Set(1, 2, 3, 4);
Assert.AreEqual("1,2,3,4", q1.ToString());
q1.Set(1, 2, 3, 4);
Assert.AreEqual("-1,-2,-3,4", q1.Reversed().ToString());
q1.Reverse();
Assert.AreEqual("-1,-2,-3,4", q1.ToString());
q1.Set(1, 2, 3, 4);
Assert.AreEqual("-0.0333333333333333,-0.0666666666666667,-0.1,0.133333333333333", q1.Inverted().ToString());
q1.Invert();
Assert.AreEqual("-0.0333333333333333,-0.0666666666666667,-0.1,0.133333333333333", q1.ToString());
Assert.AreEqual(0.0333333333333333, q1.SquareNorm(), 0.000000000001);
Assert.AreEqual(0.182574185835055, q1.Norm(), 0.000000000001);
q1.Set(1, 2, 3, 4);
Assert.AreEqual("2,4,6,8", q1.Scaled(2).ToString());
q1.Scale(2);
Assert.AreEqual("2,4,6,8", q1.ToString());
q1.Set(2, 2, 0, 1);
q1.StabilizeLength();
Assert.AreEqual("0.4,0.4,0,0.2", q1.ToString());
q1.Set(2, 2, 0, 1);
Assert.AreEqual("0.666666666666667,0.666666666666667,0,0.333333333333333", q1.Normalized().ToString());
q1.Normalize();
Assert.AreEqual("0.666666666666667,0.666666666666667,0,0.333333333333333", q1.ToString());
q1.Set(2, 2, 0, 1);
Assert.AreEqual("-2,-2,0,-1", q1.Negated().ToString());
q1.Set(1, 2, 3, 4);
Assert.AreEqual("3,5,7,9", q1.Added(new Quaternion(2, 3, 4, 5)).ToString());
q1.Add(new Quaternion(2, 3, 4, 5));
Assert.AreEqual("3,5,7,9", q1.ToString());
q1.Set(1, 2, 3, 4);
Assert.AreEqual("3,5,7,9", q1.Subtracted(new Quaternion(-2, -3, -4, -5)).ToString());
q1.Subtract(new Quaternion(-2, -3, -4, -5));
Assert.AreEqual("3,5,7,9", q1.ToString());
q1.Set(1, 2, 3, 4);
Assert.AreEqual("12,24,30,0", q1.Multiplied(new Quaternion(2, 3, 4, 5)).ToString());
q1.Multiply(new Quaternion(2, 3, 4, 5));
Assert.AreEqual("12,24,30,0", q1.ToString());
q1.Set(1, 2, 3, 1);
Assert.AreEqual(12, q1.Dot(new Quaternion(2, 2, 1, 3)));
Assert.AreEqual(2.6192778317837444, q1.GetRotationAngle(), 0.00000000000001);
q1 = new Quaternion(new Vec(1, 0, 0), Math.PI);
Assert.AreEqual("1,-2,-3", q1.Multiply(new Vec(1, 2, 3)).ToString());
//gp_Quaternion gq1 = new gp_Quaternion(new gp_Vec(1, 0, 0), Math.PI);
//TestContext.WriteLine(string.Format(CultureInfo.InvariantCulture, "{0},{1},{2},{3}", gq1.X(), gq1.Y(), gq1.Z(), gq1.W()));
}
public static Quaternion Blend(this Quaternion q1, Quaternion q2, float weight)
{
return(q1.Add(q2.Scale(weight)));
}
private void Window_Loaded(object sender, RoutedEventArgs e)
{
this.m_helix_viewport.ModelUpDirection = new Vector3D(0, 1, 0);
//helloClass X = new helloClass();
//MWArray[] result = X.hello(1);
//System.Diagnostics.Debug.WriteLine((MWNumericArray)result[0]);
populate_reference_data();
initial_frame[0] = new Quaternion(-0.330925973595304, -0.29698484809835, 0.524673231640418, 0.725491557497398);
initial_frame[1] = new Quaternion(-0.323147799002252, 0.173241161390704, 0.143542676580869, -0.918531708761325);
initial_frame[2] = new Quaternion(0.580534667354156, -0.00565685424949242, 0.410121933088198, -0.703571247280615);
initial_frame[3] = new Quaternion(0.399515331370399, -0.20081832585698, 0.526087445202791, -0.723370237153838);
//The Importer to load .obj files
ModelImporter importer = new ModelImporter();
//The Material (Color) that is applyed to the importet objects
Material material = new DiffuseMaterial(new SolidColorBrush(Colors.BurlyWood));
importer.DefaultMaterial = material;
//instanciate a new group of 3D Models
skeleton = new Model3DGroup();
waist = importer.Load(Path.Combine(components_dir, "waist.stl"));
torso = importer.Load(Path.Combine(components_dir, "rightTorso.stl"));
rightUpperArm = importer.Load(Path.Combine(components_dir, "rightUpperArm.stl"));
rightForearm = importer.Load(Path.Combine(components_dir, "rightForeArm.stl"));
rightHand = importer.Load(Path.Combine(components_dir, "rightHand.stl"));
skeleton.Children.Add(waist);
skeleton.Children.Add(torso);
skeleton.Children.Add(rightUpperArm);
skeleton.Children.Add(rightForearm);
//skeleton.Children.Add(rightHand);
components = new Model3D[] { torso, rightUpperArm, rightForearm, rightHand };
var componentTransform = new Transform3DGroup();
RotateTransform3D initalRotationZ = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 0, 1), 100));
initalRotationZ.CenterX = centroids[0][0];
initalRotationZ.CenterY = centroids[0][1];
initalRotationZ.CenterZ = centroids[0][2];
//componentTransform.Children.Add(initalRotationZ);
RotateTransform3D initalRotationX = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(1, 0, 0), 20));
initalRotationX.CenterX = centroids[0][0];
initalRotationX.CenterY = centroids[0][1];
initalRotationX.CenterZ = centroids[0][2];
//componentTransform.Children.Add(initalRotationX);
skeleton.Transform = componentTransform;
modelVisual3D = new ModelVisual3D();
modelVisual3D.Content = skeleton;
this.m_helix_viewport.Children.Add(modelVisual3D);
Avatar.DataContext = this;
Quaternion Q1 = new Quaternion(0.1, 0.6, 0.1, 0.2);
Quaternion Q2 = new Quaternion(0.3, 0.4, 0.1, 0.2);
Quaternion z = Quaternion.Add(Q1, Q2);
Quaternion Q1i = Q1; // new Quaternion(Q1.X, Q1.Y, Q1.Z, Q1.W);
Q1i.Conjugate();
Quaternion Q1i2 = Q1;
Q1i2.Invert();
double M = Math.Abs(Q1.X + Q1.Y + Q1.Z);
Quaternion X = new Quaternion(Q1i.X / M, Q1i.Y / M, Q1i.Z / M, Q1i.W / M);
Quaternion dif = Quaternion.Multiply(Q2, Q1i);
Quaternion ans = Quaternion.Multiply(Q1, dif);
//pdr = new ProcessedDataReader(input_data);
}
public void Add_Static(Quaternion left, Quaternion right, Quaternion expected)
{
var actual = Quaternion.Add(left, right);
Assert.Equal(expected, actual);
}
public Quaternion DivideBenchmark() => Quaternion.Add(Quaternion.Identity, Quaternion.Identity);
