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