public void AdditionOperator()
{
QuaternionF a = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f);
QuaternionF b = new QuaternionF(2.0f, 3.0f, 4.0f, 5.0f);
QuaternionF c = a + b;
Assert.AreEqual(new QuaternionF(3.0f, 5.0f, 7.0f, 9.0f), c);
}
public void Addition()
{
QuaternionF a = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f);
QuaternionF b = new QuaternionF(2.0f, 3.0f, 4.0f, 5.0f);
QuaternionF c = QuaternionF.Add(a, b);
Assert.AreEqual(new QuaternionF(3.0f, 5.0f, 7.0f, 9.0f), c);
}
public void AreEqualWithEpsilon()
{
float epsilon = 0.001f;
QuaternionF q1 = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f);
QuaternionF q2 = new QuaternionF(1.002f, 2.002f, 3.002f, 4.002f);
QuaternionF q3 = new QuaternionF(1.0001f, 2.0001f, 3.0001f, 4.0001f);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(q1, q1, epsilon));
Assert.IsFalse(QuaternionF.AreNumericallyEqual(q1, q2, epsilon));
Assert.IsTrue(QuaternionF.AreNumericallyEqual(q1, q3, epsilon));
}
public void AreEqual()
{
float originalEpsilon = Numeric.EpsilonF;
Numeric.EpsilonF = 1e-8f;
QuaternionF q1 = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f);
QuaternionF q2 = new QuaternionF(1.000001f, 2.000001f, 3.000001f, 4.000001f);
QuaternionF q3 = new QuaternionF(1.00000001f, 2.00000001f, 3.00000001f, 4.00000001f);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(q1, q1));
Assert.IsFalse(QuaternionF.AreNumericallyEqual(q1, q2));
Assert.IsTrue(QuaternionF.AreNumericallyEqual(q1, q3));
Numeric.EpsilonF = originalEpsilon;
}
public void ConstructorTest()
{
var rotationQ = new QuaternionF(1, 2, 3, 4).Normalized;
var srt = new SrtTransform(rotationQ.ToRotationMatrix33());
Assert.AreEqual(Vector3F.One, srt.Scale);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(rotationQ, srt.Rotation));
Assert.AreEqual(Vector3F.Zero, srt.Translation);
srt = new SrtTransform(rotationQ);
Assert.AreEqual(Vector3F.One, srt.Scale);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(rotationQ, srt.Rotation));
Assert.AreEqual(Vector3F.Zero, srt.Translation);
srt = new SrtTransform(new Vector3F(-1, 2, -3), rotationQ.ToRotationMatrix33(), new Vector3F(10, 9, -8));
Assert.AreEqual(new Vector3F(-1, 2, -3), srt.Scale);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(rotationQ, srt.Rotation));
Assert.AreEqual(new Vector3F(10, 9, -8), srt.Translation);
}
public static bool ApproximateEquals(this QuaternionF q0, QuaternionF q1)
{
return(ApproximateEquals(q0, q1, Constant <float> .PositiveTinyValue));
}
public BoneAnimationFrame(Bone bone, CoordinateF position, QuaternionF angles)
{
Bone = bone;
Position = position;
Angles = angles;
}
// Handle character-related input and move the character.
private void ControlCharacter(float deltaTime)
{
// Compute new orientation from mouse movement.
float deltaYaw = -InputService.MousePositionDelta.X;
_yaw += deltaYaw * deltaTime * 0.1f;
float deltaPitch = -InputService.MousePositionDelta.Y;
_pitch += deltaPitch * deltaTime * 0.1f;
// Limit the pitch angle.
_pitch = MathHelper.Clamp(_pitch, -ConstantsF.PiOver2, ConstantsF.PiOver2);
// Compute new orientation of the camera.
QuaternionF cameraOrientation = QuaternionF.CreateRotationY(_yaw) * QuaternionF.CreateRotationX(_pitch);
// Create velocity from WASD keys.
// TODO: Diagonal movement is faster ;-). Fix this.
Vector3F velocityVector = Vector3F.Zero;
if (Keyboard.GetState().IsKeyDown(Keys.W))
{
velocityVector.Z--;
}
if (Keyboard.GetState().IsKeyDown(Keys.S))
{
velocityVector.Z++;
}
if (Keyboard.GetState().IsKeyDown(Keys.A))
{
velocityVector.X--;
}
if (Keyboard.GetState().IsKeyDown(Keys.D))
{
velocityVector.X++;
}
velocityVector *= 10 * deltaTime;
// Velocity vector is currently in view space. -z is the forward direction.
// We have to convert this vector to world space by rotating it.
velocityVector = QuaternionF.CreateRotationY(_yaw).Rotate(velocityVector);
// New compute desired character controller position in world space:
Vector3F targetPosition = _character.Position + velocityVector;
// Check if user wants to jump.
bool jump = Keyboard.GetState().IsKeyDown(Keys.Space);
// Call character controller to compute a new valid position. The character
// controller slides along obstacles, handles stepping up/down, etc.
_character.Move(targetPosition, deltaTime, jump);
// ----- Set view matrix for graphics.
// For third person we move the eye position back, behind the body (+z direction is
// the "back" direction).
Vector3F thirdPersonDistance = cameraOrientation.Rotate(new Vector3F(0, 0, 6));
// Compute camera pose (= position + orientation).
_cameraNode.PoseWorld = new Pose
{
Position = _character.Position // Floor position of character
+ new Vector3F(0, 1.6f, 0) // + Eye height
+ thirdPersonDistance,
Orientation = cameraOrientation.ToRotationMatrix33()
};
}
public void LnException()
{
QuaternionF q = new QuaternionF(1.5f, 0.0f, 0.0f, 0.0f);
QuaternionF.Ln(q);
}
public void GetValueTest()
{
var animation = new AnimationClip <float>
{
Animation = new SingleFromToByAnimation
{
From = 100,
To = 200,
Duration = TimeSpan.FromSeconds(6.0),
},
Delay = TimeSpan.FromSeconds(10),
Speed = 2,
FillBehavior = FillBehavior.Hold,
};
var animation2 = new AnimationClip <float>
{
Animation = new SingleFromToByAnimation
{
From = 10,
To = 20,
Duration = TimeSpan.FromSeconds(5.0),
},
Delay = TimeSpan.FromSeconds(0),
Speed = 1,
FillBehavior = FillBehavior.Hold,
};
var animation3 = new AnimationClip <float>
{
Animation = new SingleFromToByAnimation
{
From = 5,
To = -5,
Duration = TimeSpan.FromSeconds(10),
},
Delay = TimeSpan.FromSeconds(5),
Speed = 1,
FillBehavior = FillBehavior.Hold,
};
var animation4 = new AnimationClip <float>
{
Animation = new SingleFromToByAnimation
{
From = 1000,
To = 1100,
Duration = TimeSpan.FromSeconds(5.0),
},
Delay = TimeSpan.FromSeconds(5),
Speed = 1,
FillBehavior = FillBehavior.Stop,
};
var animationEx = new QuaternionFAnimation
{
W = animation,
X = animation2,
Y = animation3,
Z = animation4,
};
var defaultSource = new QuaternionF(1, 2, 3, 4);
var defaultTarget = new QuaternionF(5, 6, 7, 8);
var result = animationEx.GetValue(TimeSpan.FromSeconds(0.0), defaultSource, defaultTarget);
Assert.AreEqual(defaultSource.W, result.W); // animation has not started.
Assert.AreEqual(10.0f, result.X); // animation2 has started.
Assert.AreEqual(defaultSource.Y, result.Y); // animation3 has not started.
Assert.AreEqual(defaultSource.Z, result.Z); // animation4 has not started.
result = animationEx.GetValue(TimeSpan.FromSeconds(5.0), defaultSource, defaultTarget);
Assert.AreEqual(defaultSource.W, result.W); // animation has not started.
Assert.AreEqual(20.0f, result.X); // animation2 has ended.
Assert.AreEqual(5, result.Y); // animation3 has started.
Assert.AreEqual(1000, result.Z); // animation4 has started.
result = animationEx.GetValue(TimeSpan.FromSeconds(5.0), defaultSource, defaultTarget);
Assert.AreEqual(defaultSource.W, result.W); // animation has not started.
Assert.AreEqual(20.0f, result.X); // animation2 has ended.
Assert.AreEqual(5, result.Y); // animation3 has started.
Assert.AreEqual(1000, result.Z); // animation4 has started.
result = animationEx.GetValue(TimeSpan.FromSeconds(13.0), defaultSource, defaultTarget);
Assert.AreEqual(200, result.W); // animation has ended.
Assert.AreEqual(20.0f, result.X); // animation2 is filling.
Assert.AreEqual(-3, result.Y); // animation3 is active.
Assert.AreEqual(defaultSource.Z, result.Z); // animation4 is stopped.
}
/// <summary>
/// Sets the bone rotation of a bone so that it matches the given rotation in model space.
/// </summary>
/// <param name="skeletonPose">The skeleton pose.</param>
/// <param name="boneIndex">The index of the bone.</param>
/// <param name="rotation">The rotation in model space.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="skeletonPose" /> is <see langword="null"/>.
/// </exception>
public static void SetBoneRotationAbsolute(this SkeletonPose skeletonPose, int boneIndex, QuaternionF rotation)
{
if (skeletonPose == null)
throw new ArgumentNullException("skeletonPose");
var skeleton = skeletonPose.Skeleton;
var bindPoseRelative = skeleton.GetBindPoseRelative(boneIndex);
var parentIndex = skeleton.GetParent(boneIndex);
var parentBonePoseAbsolute = (parentIndex >= 0) ? skeletonPose.GetBonePoseAbsolute(parentIndex) : SrtTransform.Identity;
// Solving this equation (using only rotations):
// rotation = parentBonePoseAbsolute * bindPoseRelative * rotationRelative;
// rotationRelative = boneTransform.
var rotationRelative = bindPoseRelative.Rotation.Conjugated
* parentBonePoseAbsolute.Rotation.Conjugated
* rotation;
rotationRelative.Normalize();
var boneTransform = skeletonPose.GetBoneTransform(boneIndex);
boneTransform.Rotation = rotationRelative;
skeletonPose.SetBoneTransform(boneIndex, boneTransform);
}
public void Power2()
{
const float θ = 0.4f;
const float t = -1.2f;
Vector3F v = new Vector3F(2.3f, 1.0f, -2.0f);
v.Normalize();
QuaternionF q = new QuaternionF((float)Math.Cos(θ), (float)Math.Sin(θ) * v);
q.Power(t);
QuaternionF expected = new QuaternionF((float)Math.Cos(t * θ), (float)Math.Sin(t * θ) * v);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(expected, q));
}
public ConstraintCarSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Create a material with high friction - this will be used for the ground and the wheels
// to give the wheels some traction.
UniformMaterial roughMaterial = new UniformMaterial
{
DynamicFriction = 1,
StaticFriction = 1,
};
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0), null, roughMaterial)
{
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(groundPlane);
// Now, we build a car out of one box for the chassis and 4 cylindric wheels.
// Front wheels are fixed with Hinge2Joints and motorized with AngularVelocityMotors.
// Back wheels are fixed with HingeJoints and not motorized. - This creates a sloppy
// car configuration. Please note that cars for racing games are not normally built with
// simple constraints - nevertheless, it is funny to do and play with it.
// Check out the "Vehicle Sample" (not included in this project)! The "Vehicle Sample"
// provides a robust ray-car implementation.)
// ----- Chassis
BoxShape chassisShape = new BoxShape(1.7f, 1, 4f);
MassFrame chassisMass = MassFrame.FromShapeAndDensity(chassisShape, Vector3F.One, 200, 0.01f, 3);
// Here is a trick: The car topples over very easily. By lowering the center of mass we
// make it more stable.
chassisMass.Pose = new Pose(new Vector3F(0, -1, 0));
RigidBody chassis = new RigidBody(chassisShape, chassisMass, null)
{
Pose = new Pose(new Vector3F(0, 1, 0)),
};
Simulation.RigidBodies.Add(chassis);
// ------ Wheels
CylinderShape cylinderShape = new CylinderShape(0.4f, 0.3f);
MassFrame wheelMass = MassFrame.FromShapeAndDensity(cylinderShape, Vector3F.One, 500, 0.01f, 3);
RigidBody wheelFrontLeft = new RigidBody(cylinderShape, wheelMass, roughMaterial)
{
Pose = new Pose(new Vector3F(0, 1, 0), Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(wheelFrontLeft);
RigidBody wheelFrontRight = new RigidBody(cylinderShape, wheelMass, roughMaterial)
{
Pose = new Pose(new Vector3F(0, 1, 0), Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(wheelFrontRight);
RigidBody wheelBackLeft = new RigidBody(cylinderShape, wheelMass, roughMaterial)
{
Pose = new Pose(new Vector3F(0, 1, 0), Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(wheelBackLeft);
RigidBody wheelBackRight = new RigidBody(cylinderShape, wheelMass, roughMaterial)
{
Pose = new Pose(new Vector3F(0, 1, 0), Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(wheelBackRight);
// ----- Hinge2Joints for the front wheels.
// A Hinge2Joint allows a limited rotation on the first constraint axis - the steering
// axis. The second constraint axis is locked and the third constraint axis is the
// rotation axis of the wheels.
_frontLeftHinge = new Hinge2Joint
{
BodyA = chassis,
// --> To define the constraint anchor orientation for the chassis:
// The columns are the axes. We set the local y axis in the first column. This is
// steering axis. In the last column we set the -x axis. This is the wheel rotation axis.
// In the middle column is a vector that is normal to the first and last axis.
// (All three columns are orthonormal and form a valid rotation matrix.)
AnchorPoseALocal = new Pose(new Vector3F(-0.9f, -0.4f, -1.4f),
new Matrix33F(0, 0, -1,
1, 0, 0,
0, -1, 0)),
BodyB = wheelFrontLeft,
// --> To define the constraint anchor orientation for the chassis:
// The columns are the axes. We set the local x axis in the first column. This is
// steering axis. In the last column we set the y axis. This is the wheel rotation axis.
// (In local space of a cylinder the cylinder axis is the +y axis.)
// In the middle column is a vector that is normal to the first and last axis.
// (All three columns are orthonormal and form a valid rotation matrix.)
AnchorPoseBLocal = new Pose(new Matrix33F(1, 0, 0,
0, 0, 1,
0, -1, 0)),
CollisionEnabled = false,
Minimum = new Vector2F(-0.7f, float.NegativeInfinity),
Maximum = new Vector2F(0.7f, float.PositiveInfinity),
};
Simulation.Constraints.Add(_frontLeftHinge);
_frontRightHinge = new Hinge2Joint
{
BodyA = chassis,
BodyB = wheelFrontRight,
AnchorPoseALocal = new Pose(new Vector3F(0.9f, -0.4f, -1.4f),
new Matrix33F(0, 0, -1,
1, 0, 0,
0, -1, 0)),
AnchorPoseBLocal = new Pose(new Matrix33F(1, 0, 0,
0, 0, 1,
0, -1, 0)),
CollisionEnabled = false,
Minimum = new Vector2F(-0.7f, float.NegativeInfinity),
Maximum = new Vector2F(0.7f, float.PositiveInfinity),
};
Simulation.Constraints.Add(_frontRightHinge);
// ----- HingeJoints for the back wheels.
// Hinges allow free rotation on the first constraint axis.
HingeJoint backLeftHinge = new HingeJoint
{
BodyA = chassis,
AnchorPoseALocal = new Pose(new Vector3F(-0.9f, -0.4f, 1.4f)),
BodyB = wheelBackLeft,
// --> To define the constraint anchor orientation:
// The columns are the axes. We set the local y axis in the first column. This is
// cylinder axis and should be the hinge axis. In the other two columns we set two
// orthonormal vectors.
// (All three columns are orthonormal and form a valid rotation matrix.)
AnchorPoseBLocal = new Pose(new Matrix33F(0, 0, 1,
1, 0, 0,
0, 1, 0)),
CollisionEnabled = false,
};
Simulation.Constraints.Add(backLeftHinge);
HingeJoint backRightHinge = new HingeJoint
{
BodyA = chassis,
AnchorPoseALocal = new Pose(new Vector3F(0.9f, -0.4f, 1.4f)),
BodyB = wheelBackRight,
AnchorPoseBLocal = new Pose(new Matrix33F(0, 0, 1,
1, 0, 0,
0, 1, 0)),
CollisionEnabled = false,
};
Simulation.Constraints.Add(backRightHinge);
// ----- Motors for the front wheels.
// (Motor axes and target velocities are set in Update() below.)
_frontLeftMotor = new AngularVelocityMotor
{
BodyA = chassis,
BodyB = wheelFrontLeft,
CollisionEnabled = false,
// We use "single axis mode", which means the motor drives only on axis and does not
// block motion orthogonal to this axis. - Rotation about the orthogonal axes is already
// controlled by the Hinge2Joint.
UseSingleAxisMode = true,
// The motor has only limited power:
MaxForce = 50000,
};
Simulation.Constraints.Add(_frontLeftMotor);
_frontRightMotor = new AngularVelocityMotor
{
BodyA = chassis,
BodyB = wheelFrontRight,
CollisionEnabled = false,
UseSingleAxisMode = true,
MaxForce = 50000,
};
Simulation.Constraints.Add(_frontRightMotor);
// ----- Drop a few boxes to create obstacles.
BoxShape boxShape = new BoxShape(1, 1, 1);
MassFrame boxMass = MassFrame.FromShapeAndDensity(boxShape, Vector3F.One, 100, 0.01f, 3);
for (int i = 0; i < 20; i++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-20, 20);
position.Y = 5;
QuaternionF orientation = RandomHelper.Random.NextQuaternionF();
RigidBody body = new RigidBody(boxShape, boxMass, null)
{
Pose = new Pose(position, orientation),
};
Simulation.RigidBodies.Add(body);
}
}
public void NegationOperator()
{
QuaternionF a = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f);
Assert.AreEqual(new QuaternionF(-1.0f, -2.0f, -3.0f, -4.0f), -a);
}
public void Normalized()
{
QuaternionF q = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f);
Assert.AreNotEqual(1.0f, q.Modulus);
Assert.IsFalse(q.IsNumericallyNormalized);
QuaternionF normalized = q.Normalized;
Assert.AreEqual(new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f), q);
Assert.IsTrue(Numeric.AreEqual(1.0f, normalized.Modulus));
Assert.IsTrue(normalized.IsNumericallyNormalized);
}
public void Negation()
{
QuaternionF a = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f);
Assert.AreEqual(new QuaternionF(-1.0f, -2.0f, -3.0f, -4.0f), QuaternionF.Negate(a));
}
public void MultiplyScalarOperator()
{
float s = 123.456f;
QuaternionF q = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f);
QuaternionF expectedResult = new QuaternionF(s * 1.0f, s * 2.0f, s * 3.0f, s * 4.0f);
QuaternionF result1 = s * q;
QuaternionF result2 = q * s;
Assert.AreEqual(expectedResult, result1);
Assert.AreEqual(expectedResult, result2);
}
public void MultiplyScalar()
{
float s = 123.456f;
QuaternionF q = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f);
QuaternionF expectedResult = new QuaternionF(s * 1.0f, s * 2.0f, s * 3.0f, s * 4.0f);
QuaternionF result = QuaternionF.Multiply(s, q);
Assert.AreEqual(expectedResult, result);
}
/// <summary>
/// Called when <see cref="IKSolver.Solve"/> is called.
/// </summary>
/// <param name="deltaTime">The current time step (in seconds).</param>
protected override void OnSolve(float deltaTime)
{
if (_isDirty)
{
// Validate bone chain.
if (!SkeletonPose.IsAncestorOrSelf(RootBoneIndex, HingeBoneIndex))
{
throw new ArgumentException("The RootBoneIndex and the HingeBoneIndex do not form a valid bone chain.");
}
if (!SkeletonPose.IsAncestorOrSelf(HingeBoneIndex, TipBoneIndex))
{
throw new ArgumentException("The HingeBoneIndex and the TipBoneIndex do not form a valid bone chain.");
}
}
_isDirty = false;
if (MinHingeAngle > MaxHingeAngle)
{
throw new AnimationException("The MinHingeAngle must be less than or equal to the MaxHingeAngle");
}
// Remember original bone transforms for interpolation at the end.
var originalRootBoneTransform = SkeletonPose.GetBoneTransform(RootBoneIndex);
var originalHingeBoneTransform = SkeletonPose.GetBoneTransform(HingeBoneIndex);
var originalTipBoneTransform = SkeletonPose.GetBoneTransform(TipBoneIndex);
var rootBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(RootBoneIndex);
var hingeBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(HingeBoneIndex);
var tipBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex);
// Get tip position in model space.
Vector3F tipAbsolute;
if (TipBoneOrientation != null)
{
// If the user has specified an absolute tip rotation, then we consider this rotation and
// use the tip bone origin as tip.
tipAbsolute = tipBonePoseAbsolute.Translation;
Target -= tipBonePoseAbsolute.ToParentDirection(tipBonePoseAbsolute.Scale * TipOffset);
}
else
{
// The user hasn't specified a desired tip rotation. Therefore we do not modify the
// tip rotation and use the offset position in the tip bone as tip.
tipAbsolute = tipBonePoseAbsolute.ToParentPosition(TipOffset);
}
// Abort if we already touch the target.
if (Vector3F.AreNumericallyEqual(tipAbsolute, Target))
{
return;
}
// Root to target vector.
var rootToTarget = Target - rootBonePoseAbsolute.Translation;
var rootToTargetLength = rootToTarget.Length;
if (Numeric.IsZero(rootToTargetLength))
{
return;
}
rootToTarget /= rootToTargetLength;
// ----- Align chain with target.
// Align the root to target vector with the root to tip vector.
var rootToTip = tipAbsolute - rootBonePoseAbsolute.Translation;
var rootToTipLength = rootToTip.Length;
if (!Numeric.IsZero(rootToTipLength))
{
rootToTip /= rootToTipLength;
var rotation = QuaternionF.CreateRotation(rootToTip, rootToTarget);
if (rotation.Angle > Numeric.EpsilonF)
{
// Apply rotation to root bone.
rootBonePoseAbsolute.Rotation = rotation * rootBonePoseAbsolute.Rotation;
SkeletonPose.SetBoneRotationAbsolute(RootBoneIndex, rootBonePoseAbsolute.Rotation);
hingeBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(HingeBoneIndex);
// Compute new tip absolute tip position from the known quantities.
tipAbsolute = rootBonePoseAbsolute.Translation + rootToTarget * rootToTipLength;
}
}
// ----- Compute ideal angle.
var rootToHinge = hingeBonePoseAbsolute.Translation - rootBonePoseAbsolute.Translation;
var hingeToTip = tipAbsolute - hingeBonePoseAbsolute.Translation;
var hingeAxis = hingeBonePoseAbsolute.ToParentDirection(HingeAxis);
// Project vectors to hinge plane. Everything should be in a plane for the following
// computations.
rootToHinge -= hingeAxis * Vector3F.Dot(rootToHinge, hingeAxis);
hingeToTip -= hingeAxis * Vector3F.Dot(hingeToTip, hingeAxis);
// Get lengths.
float rootToHingeLength = rootToHinge.Length;
if (Numeric.IsZero(rootToHingeLength))
{
return;
}
rootToHinge /= rootToHingeLength;
float hingeToTipLength = hingeToTip.Length;
if (Numeric.IsZero(hingeToTipLength))
{
return;
}
hingeToTip /= hingeToTipLength;
// Compute current hinge angle (angle between root bone and hinge bone).
float currentHingeAngle = (float)Math.Acos(MathHelper.Clamp(Vector3F.Dot(rootToHinge, hingeToTip), -1, 1));
// Make sure the computed angle is about the hingeAxis and not about -hingeAxis.
if (Vector3F.Dot(Vector3F.Cross(rootToHinge, hingeToTip), hingeAxis) < 0)
{
currentHingeAngle = -currentHingeAngle;
}
// Using law of cosines to compute the desired hinge angle using the triangle lengths.
float cosDesiredHingeAngle = (rootToHingeLength * rootToHingeLength + hingeToTipLength * hingeToTipLength - rootToTargetLength * rootToTargetLength)
/ (2 * rootToHingeLength * hingeToTipLength);
float desiredHingeAngle = ConstantsF.Pi - (float)Math.Acos(MathHelper.Clamp(cosDesiredHingeAngle, -1, 1));
// Apply hinge limits.
if (desiredHingeAngle < MinHingeAngle)
{
desiredHingeAngle = MinHingeAngle;
}
else if (desiredHingeAngle > MaxHingeAngle)
{
desiredHingeAngle = MaxHingeAngle;
}
// Compute delta rotation between current and desired angle.
float deltaAngle = desiredHingeAngle - currentHingeAngle;
var hingeRotation = QuaternionF.CreateRotation(hingeAxis, deltaAngle);
hingeBonePoseAbsolute.Rotation = hingeRotation * hingeBonePoseAbsolute.Rotation;
// Update tip position.
tipAbsolute = hingeBonePoseAbsolute.Translation + hingeRotation.Rotate(tipAbsolute - hingeBonePoseAbsolute.Translation);
// ----- Align chain with target.
// If we hit a hinge limit, then we can move the tip closer to the target by aligning
// the whole chain again.
rootToTip = tipAbsolute - rootBonePoseAbsolute.Translation;
rootToTipLength = rootToTip.Length;
if (!Numeric.IsZero(rootToTipLength))
{
rootToTip /= rootToTipLength;
var rotation = QuaternionF.CreateRotation(rootToTip, rootToTarget);
rootBonePoseAbsolute.Rotation = rotation * rootBonePoseAbsolute.Rotation;
hingeBonePoseAbsolute.Rotation = rotation * hingeBonePoseAbsolute.Rotation;
}
// ----- Set results.
SkeletonPose.SetBoneRotationAbsolute(RootBoneIndex, rootBonePoseAbsolute.Rotation);
SkeletonPose.SetBoneRotationAbsolute(HingeBoneIndex, hingeBonePoseAbsolute.Rotation);
if (TipBoneOrientation != null)
{
SkeletonPose.SetBoneRotationAbsolute(TipBoneIndex, TipBoneOrientation.Value);
}
// ----- Apply weight, velocity limit and set results.
bool requiresBlending = RequiresBlending();
float maxRotationAngle;
bool requiresLimiting = RequiresLimiting(deltaTime, out maxRotationAngle);
if (requiresBlending || requiresLimiting)
{
var targetBoneTransform = SkeletonPose.GetBoneTransform(RootBoneIndex);
if (requiresBlending)
{
BlendBoneTransform(ref originalRootBoneTransform, ref targetBoneTransform);
}
if (requiresLimiting)
{
LimitBoneTransform(ref originalRootBoneTransform, ref targetBoneTransform, maxRotationAngle);
}
SkeletonPose.SetBoneTransform(RootBoneIndex, targetBoneTransform);
targetBoneTransform = SkeletonPose.GetBoneTransform(HingeBoneIndex);
if (requiresBlending)
{
BlendBoneTransform(ref originalHingeBoneTransform, ref targetBoneTransform);
}
if (requiresLimiting)
{
LimitBoneTransform(ref originalHingeBoneTransform, ref targetBoneTransform, maxRotationAngle);
}
SkeletonPose.SetBoneTransform(HingeBoneIndex, targetBoneTransform);
targetBoneTransform = SkeletonPose.GetBoneTransform(TipBoneIndex);
if (requiresBlending)
{
BlendBoneTransform(ref originalTipBoneTransform, ref targetBoneTransform);
}
if (requiresLimiting)
{
LimitBoneTransform(ref originalTipBoneTransform, ref targetBoneTransform, maxRotationAngle);
}
SkeletonPose.SetBoneTransform(TipBoneIndex, targetBoneTransform);
}
}
public void Power3()
{
const float θ = 0.4f;
Vector3F v = new Vector3F(2.3f, 1.0f, -2.0f);
v.Normalize();
QuaternionF q = new QuaternionF((float)Math.Cos(θ), (float)Math.Sin(θ) * v);
QuaternionF q2 = q;
q2.Power(2);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(q * q, q2));
QuaternionF q3 = q;
q3.Power(3);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(q * q * q, q3));
q2 = q;
q2.Power(-2);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(q.Inverse * q.Inverse, q2));
q3 = q;
q3.Power(-3);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(q.Inverse * q.Inverse * q.Inverse, q3));
}
protected override void OnUpdate(TimeSpan deltaTime)
{
// Mouse centering (controlled by the MenuComponent) is disabled if the game
// is inactive or if the GUI is active. In these cases, we do not want to move
// the player.
if (!_inputService.EnableMouseCentering)
{
return;
}
float deltaTimeF = (float)deltaTime.TotalSeconds;
// ----- Hulk Mode
// Toggle "Hulk" mode if <H> or <X> (gamepad) is pressed.
if (_inputService.IsPressed(Keys.H, false) || _inputService.IsPressed(Buttons.X, false, LogicalPlayerIndex.One))
{
ToggleHulk();
}
// ----- Crouching
if (_inputService.IsPressed(Keys.LeftShift, false) || _inputService.IsPressed(Buttons.RightTrigger, false, LogicalPlayerIndex.One))
{
Crouch();
}
else if (!_inputService.IsDown(Keys.LeftShift) && !_inputService.IsDown(Buttons.RightTrigger, LogicalPlayerIndex.One) && CharacterController.Height <= 1)
{
StandUp();
}
// ----- Update orientation
// Update _yaw and _pitch.
UpdateOrientation(deltaTimeF);
// Compute the new orientation of the camera.
QuaternionF orientation = QuaternionF.CreateRotationY(_yaw) * QuaternionF.CreateRotationX(_pitch);
// ----- Compute translation
// Create velocity from <W>, <A>, <S>, <D> and gamepad sticks.
Vector3F moveDirection = Vector3F.Zero;
if (Keyboard.GetState().IsKeyDown(Keys.W))
{
moveDirection.Z--;
}
if (Keyboard.GetState().IsKeyDown(Keys.S))
{
moveDirection.Z++;
}
if (Keyboard.GetState().IsKeyDown(Keys.A))
{
moveDirection.X--;
}
if (Keyboard.GetState().IsKeyDown(Keys.D))
{
moveDirection.X++;
}
moveDirection.X += _inputService.GetGamePadState(LogicalPlayerIndex.One).ThumbSticks.Left.X;
moveDirection.Z -= _inputService.GetGamePadState(LogicalPlayerIndex.One).ThumbSticks.Left.Y;
// Rotate the velocity vector from view space to world space.
moveDirection = orientation.Rotate(moveDirection);
// Add velocity from <R>, <F> keys.
// <R> or DPad up is used to move up ("rise").
// <F> or DPad down is used to move down ("fall").
if (Keyboard.GetState().IsKeyDown(Keys.R) || _inputService.GetGamePadState(LogicalPlayerIndex.One).DPad.Up == ButtonState.Pressed)
{
moveDirection.Y++;
}
if (Keyboard.GetState().IsKeyDown(Keys.F) || _inputService.GetGamePadState(LogicalPlayerIndex.One).DPad.Down == ButtonState.Pressed)
{
moveDirection.Y--;
}
// ----- Climbing
bool hasLadderContact = HasLadderContact();
bool hasLedgeContact = HasLedgeContact();
CharacterController.IsClimbing = hasLadderContact || hasLedgeContact;
// When the character is walking (gravity > 0) it cannot walk up/down - only on a ladder.
if (CharacterController.Gravity != 0 && !hasLadderContact)
{
moveDirection.Y = 0;
}
// ----- Moving
moveDirection.TryNormalize();
Vector3F moveVelocity = moveDirection * LinearVelocityMagnitude;
// ----- Jumping
if ((_inputService.IsPressed(Keys.Space, false) || _inputService.IsPressed(Buttons.A, false, LogicalPlayerIndex.One)) &&
(CharacterController.HasGroundContact || CharacterController.IsClimbing))
{
// Jump button was newly pressed and the character has support to start the jump.
_timeSinceLastJump = 0;
}
float jumpVelocity = 0;
if ((_inputService.IsDown(Keys.Space) || _inputService.IsDown(Buttons.A, LogicalPlayerIndex.One)))
{
// Jump button is still down.
if (_timeSinceLastJump + deltaTimeF <= DynamicJumpTime)
{
// The DynamicJumpTime has not been exceeded.
// Set a jump velocity to make the jump higher.
jumpVelocity = JumpVelocity;
}
else if (_timeSinceLastJump <= DynamicJumpTime)
{
// The jump time exceeds DynamicJumpTime in this time step.
// _timeSinceLastJump <= DynamicJumpTime
// _timeSinceLastJump + deltaTime > DynamicJumpTime
// In order to achieve exact, reproducible jump heights we need to split
// the time step:
float deltaTime0 = DynamicJumpTime - _timeSinceLastJump;
float deltaTime1 = deltaTimeF - deltaTime0;
// The first part of the movement is a jump with active jump velocity.
jumpVelocity = JumpVelocity;
_timeSinceLastJump += deltaTime0;
CharacterController.Move(moveVelocity, jumpVelocity, deltaTime0);
// The second part of the movement is a jump without jump velocity.
jumpVelocity = 0;
deltaTimeF = deltaTime1;
}
}
_timeSinceLastJump += deltaTimeF;
// ----- Move the character.
CharacterController.Move(moveVelocity, jumpVelocity, deltaTimeF);
// Draw character controller capsule.
// The character controller is also transparent The hulk is green, of course.
var color = _isHulk ? Color.DarkGreen : Color.Gray;
color.A = 128;
_debugRenderer.DrawObject(CharacterController.Body, color, false, false);
}
public void Properties()
{
QuaternionF q = new QuaternionF(0.123f, 1.0f, 2.0f, 3.0f);
Assert.AreEqual(0.123f, q.W);
Assert.AreEqual(1.0f, q.X);
Assert.AreEqual(2.0f, q.Y);
Assert.AreEqual(3.0f, q.Z);
q.W = 1.0f;
q.X = 2.0f;
q.Y = 3.0f;
q.Z = 4.0f;
Assert.AreEqual(1.0f, q.W);
Assert.AreEqual(2.0f, q.X);
Assert.AreEqual(3.0f, q.Y);
Assert.AreEqual(4.0f, q.Z);
q.V = new Vector3F(-1.0f, -2.0f, -3.0f);
Assert.AreEqual(-1.0f, q.X);
Assert.AreEqual(-2.0f, q.Y);
Assert.AreEqual(-3.0f, q.Z);
Assert.AreEqual(new Vector3F(-1.0f, -2.0f, -3.0f), q.V);
}
//public static void RotateBoneWorld(this SkeletonPose SkeletonPose, int boneIndex, QuaternionF rotation, Matrix44F world)
//{
// QuaternionF worldRotation = QuaternionF.CreateRotation(world.Minor);
// RotateBoneAbsolute(SkeletonPose, boneIndex, worldRotation.Conjugated * rotation);
//}
// TODO: This method should really be called RotateBoneLocalAnimated?
///// <summary>
///// Rotates bone where the rotation is given in the bone space.
///// </summary>
///// <param name="skeletonPose">The skeleton pose.</param>
///// <param name="boneIndex">The index of the bone.</param>
///// <param name="rotation">The rotation in bone space.</param>
///// <exception cref="ArgumentNullException">
///// <paramref name="skeletonPose" /> is <see langword="null"/>.
///// </exception>
//public static void RotateBoneLocal(this SkeletonPose skeletonPose, int boneIndex, QuaternionF rotation)
//{
// if (skeletonPose == null)
// throw new ArgumentNullException("skeletonPose");
// var boneTransform = skeletonPose.GetBoneTransform(boneIndex);
// boneTransform.Rotation = boneTransform.Rotation * rotation;
// skeletonPose.SetBoneTransform(boneIndex, boneTransform);
//}
/// <summary>
/// Rotates a bone where the rotation is given in model space.
/// </summary>
/// <param name="skeletonPose">The skeleton pose.</param>
/// <param name="boneIndex">The index of the bone.</param>
/// <param name="rotation">The rotation in model space.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="skeletonPose" /> is <see langword="null"/>.
/// </exception>
public static void RotateBoneAbsolute(this SkeletonPose skeletonPose, int boneIndex, QuaternionF rotation)
{
if (skeletonPose == null)
{
throw new ArgumentNullException("skeletonPose");
}
var skeleton = skeletonPose.Skeleton;
var boneTransform = skeletonPose.GetBoneTransform(boneIndex);
var bindPoseRelative = skeleton.GetBindPoseRelative(boneIndex);
var parentIndex = skeleton.GetParent(boneIndex);
var parentBonePoseAbsolute = skeletonPose.GetBonePoseAbsolute(parentIndex);
// Solving these equations (using only the rotations):
// rotation * bonePoseAbsolute = bonePoseAbsoluteNew
// bonePoseAbsolute = parentBonePoseAbsolute * bindPoseRelative * boneTransform.
// ...
// Rotation relative to bone bind pose space (using similarity transformation).
var rotationRelative = bindPoseRelative.Rotation.Conjugated
* parentBonePoseAbsolute.Rotation.Conjugated
* rotation
* parentBonePoseAbsolute.Rotation
* bindPoseRelative.Rotation;
// The final rotation is: First rotate into bone bind pose space, then apply rotation.
boneTransform.Rotation = rotationRelative * boneTransform.Rotation;
// So many multiplications, numerical errors adds up quickly in iterative IK algorithms...
boneTransform.Rotation.Normalize();
skeletonPose.SetBoneTransform(boneIndex, boneTransform);
}
private static void DoWork(QuaternionF skeletonOffset, SkeletonPose skeletonA, SkeletonPose skeletonB, int boneIndexA, int neckBoneIndexA, int leftShoulderBoneIndexA, int rightShoulderBoneIndexA, int boneIndexB, int neckBoneIndexB, int leftShoulderBoneIndexB, int rightShoulderBoneIndexB)
{
// Reset root bone.
skeletonB.ResetBoneTransforms(boneIndexB, boneIndexB, false, true, false);
// Get absolute positions all bones.
var boneA = skeletonA.GetBonePoseAbsolute(boneIndexA).Translation;
var neckA = skeletonA.GetBonePoseAbsolute(neckBoneIndexA).Translation;
var leftShoulderA = skeletonA.GetBonePoseAbsolute(leftShoulderBoneIndexA).Translation;
var rightShoulderA = skeletonA.GetBonePoseAbsolute(rightShoulderBoneIndexA).Translation;
var boneB = skeletonB.GetBonePoseAbsolute(boneIndexB).Translation;
var neckB = skeletonB.GetBonePoseAbsolute(neckBoneIndexB).Translation;
var leftShoulderB = skeletonB.GetBonePoseAbsolute(leftShoulderBoneIndexB).Translation;
var rightShoulderB = skeletonB.GetBonePoseAbsolute(rightShoulderBoneIndexB).Translation;
// Abort if any bone to bone distance is 0.
if (Vector3F.AreNumericallyEqual(boneA, neckA)
|| Vector3F.AreNumericallyEqual(boneA, rightShoulderA)
|| Vector3F.AreNumericallyEqual(leftShoulderA, rightShoulderA)
|| Vector3F.AreNumericallyEqual(boneB, neckB)
|| Vector3F.AreNumericallyEqual(boneB, rightShoulderB)
|| Vector3F.AreNumericallyEqual(leftShoulderB, rightShoulderB))
{
return;
}
// Get shoulder axis vectors in model B space.
var shoulderAxisA = rightShoulderA - leftShoulderA;
shoulderAxisA = skeletonOffset.Rotate(shoulderAxisA);
var shoulderAxisB = rightShoulderB - leftShoulderB;
// Create a twist rotation from the shoulder vectors.
var shoulderRotation = QuaternionF.CreateRotation(shoulderAxisB, shoulderAxisA);
// Apply this twist to the spine. (Modifies the neckB position.)
neckB = boneB + shoulderRotation.Rotate(neckB - boneB);
// Get spine vectors in model B space.
var spineAxisA = neckA - boneA;
spineAxisA = skeletonOffset.Rotate(spineAxisA);
var spineAxisB = neckB - boneB;
// Create swing rotation from spine vectors.
var spineRotation = QuaternionF.CreateRotation(spineAxisB, spineAxisA);
// Apply the shoulder twist rotation followed by the spine swing rotation.
skeletonB.RotateBoneAbsolute(boneIndexB, spineRotation * shoulderRotation);
}
protected static double CalculateYaw(QuaternionF orientation)
{
//Contract.Requires<ArgumentNullException>(orientation != null, "orientation");
return(System.Math.Atan2(2.0f * (orientation.W * orientation.Z + orientation.X * orientation.Y), 1.0f - 2.0f * (orientation.Y * orientation.Y + orientation.Z * orientation.Z)));
}
public void Update(TimeSpan deltaTime)
{
// Update the position based upon keyboard
_orientation = QuaternionF.CreateRotationY(_yaw) * QuaternionF.CreateRotationX(_pitch);
_position.X = MathHelper.Clamp(_position.X, -170.0f, 170.0f);
_position.Y = MathHelper.Clamp(_position.Y, 40.0f, 200.0f);
_position.Z = MathHelper.Clamp(_position.Z, -170.0f, 170.0f);
_cameraNode.PoseWorld = new Pose(_position, _orientation);
}
private void OnXyzwChanged()
{
if (_isUpdating)
return;
_isUpdating = true;
try
{
if (_vectorType == typeof(Vector4))
Value = new Vector4((float)X, (float)Y, (float)Z, (float)W);
else if (_vectorType == typeof(Quaternion))
Value = new Quaternion((float)X, (float)Y, (float)Z, (float)W);
else if (_vectorType == typeof(Vector4F))
Value = new Vector4F((float)X, (float)Y, (float)Z, (float)W);
else if (_vectorType == typeof(Vector4D))
Value = new Vector4D(X, Y, Z, W);
else if (_vectorType == typeof(QuaternionF))
Value = new QuaternionF((float)W, (float)X, (float)Y, (float)Z);
else if (_vectorType == typeof(QuaternionD))
Value = new QuaternionD(W, X, Y, Z);
}
finally
{
_isUpdating = false;
}
}
public void GetValueTest()
{
var animation = new AnimationClip<float>
{
Animation = new SingleFromToByAnimation
{
From = 100,
To = 200,
Duration = TimeSpan.FromSeconds(6.0),
},
Delay = TimeSpan.FromSeconds(10),
Speed = 2,
FillBehavior = FillBehavior.Hold,
};
var animation2 = new AnimationClip<float>
{
Animation = new SingleFromToByAnimation
{
From = 10,
To = 20,
Duration = TimeSpan.FromSeconds(5.0),
},
Delay = TimeSpan.FromSeconds(0),
Speed = 1,
FillBehavior = FillBehavior.Hold,
};
var animation3 = new AnimationClip<float>
{
Animation = new SingleFromToByAnimation
{
From = 5,
To = -5,
Duration = TimeSpan.FromSeconds(10),
},
Delay = TimeSpan.FromSeconds(5),
Speed = 1,
FillBehavior = FillBehavior.Hold,
};
var animation4 = new AnimationClip<float>
{
Animation = new SingleFromToByAnimation
{
From = 1000,
To = 1100,
Duration = TimeSpan.FromSeconds(5.0),
},
Delay = TimeSpan.FromSeconds(5),
Speed = 1,
FillBehavior = FillBehavior.Stop,
};
var animationEx = new QuaternionFAnimation
{
W = animation,
X = animation2,
Y = animation3,
Z = animation4,
};
var defaultSource = new QuaternionF(1, 2, 3, 4);
var defaultTarget = new QuaternionF(5, 6, 7, 8);
var result = animationEx.GetValue(TimeSpan.FromSeconds(0.0), defaultSource, defaultTarget);
Assert.AreEqual(defaultSource.W, result.W); // animation has not started.
Assert.AreEqual(10.0f, result.X); // animation2 has started.
Assert.AreEqual(defaultSource.Y, result.Y); // animation3 has not started.
Assert.AreEqual(defaultSource.Z, result.Z); // animation4 has not started.
result = animationEx.GetValue(TimeSpan.FromSeconds(5.0), defaultSource, defaultTarget);
Assert.AreEqual(defaultSource.W, result.W); // animation has not started.
Assert.AreEqual(20.0f, result.X); // animation2 has ended.
Assert.AreEqual(5, result.Y); // animation3 has started.
Assert.AreEqual(1000, result.Z); // animation4 has started.
result = animationEx.GetValue(TimeSpan.FromSeconds(5.0), defaultSource, defaultTarget);
Assert.AreEqual(defaultSource.W, result.W); // animation has not started.
Assert.AreEqual(20.0f, result.X); // animation2 has ended.
Assert.AreEqual(5, result.Y); // animation3 has started.
Assert.AreEqual(1000, result.Z); // animation4 has started.
result = animationEx.GetValue(TimeSpan.FromSeconds(13.0), defaultSource, defaultTarget);
Assert.AreEqual(200, result.W); // animation has ended.
Assert.AreEqual(20.0f, result.X); // animation2 is filling.
Assert.AreEqual(-3, result.Y); // animation3 is active.
Assert.AreEqual(defaultSource.Z, result.Z); // animation4 is stopped.
}
public void FromToMatrixTest()
{
var t = new Vector3F(1, 2, 3);
var r = new QuaternionF(1, 2, 3, 4).Normalized;
var s = new Vector3F(2, 7, 9);
var m = Matrix44F.CreateTranslation(t) * Matrix44F.CreateRotation(r) * Matrix44F.CreateScale(s);
var srt = SrtTransform.FromMatrix(m);
Assert.IsTrue(Vector3F.AreNumericallyEqual(t, srt.Translation));
Assert.IsTrue(QuaternionF.AreNumericallyEqual(r, srt.Rotation));
Assert.IsTrue(Vector3F.AreNumericallyEqual(s, srt.Scale));
// XNA:
srt = SrtTransform.FromMatrix((Matrix)m);
Assert.IsTrue(Vector3F.AreNumericallyEqual(t, srt.Translation));
Assert.IsTrue(QuaternionF.AreNumericallyEqual(r, srt.Rotation));
Assert.IsTrue(Vector3F.AreNumericallyEqual(s, srt.Scale));
// With negative scale, the decomposition is not unique (many possible combinations of
// axis mirroring + rotation).
t = new Vector3F(1, 2, 3);
r = new QuaternionF(1, 2, 3, 4).Normalized;
s = new Vector3F(2, -7, 9);
m = Matrix44F.CreateTranslation(t) * Matrix44F.CreateRotation(r) * Matrix44F.CreateScale(s);
srt = SrtTransform.FromMatrix(m);
var m2 = (Matrix44F)srt;
Assert.IsTrue(Matrix44F.AreNumericallyEqual(m, m2));
m2 = srt.ToMatrix44F();
Assert.IsTrue(Matrix44F.AreNumericallyEqual(m, m2));
m2 = srt;
Assert.IsTrue(Matrix44F.AreNumericallyEqual(m, m2));
Matrix mXna = srt.ToXna();
Assert.IsTrue(Matrix44F.AreNumericallyEqual(m, (Matrix44F)mXna));
mXna = srt;
Assert.IsTrue(Matrix44F.AreNumericallyEqual(m, (Matrix44F)mXna));
}
/// <summary>
/// Perform mapping in absolute space.
/// </summary>
private static void MapAbsolute(bool mapTranslations, SkeletonPose skeletonA, int boneIndexA, SkeletonPose skeletonB, int boneIndexB, float scaleAToB, QuaternionF rotationBToA, QuaternionF rotationAToB)
{
// The current absolute bone pose of bone A.
var boneAActualRotationAbsolute = skeletonA.GetBonePoseAbsolute(boneIndexA).Rotation;
var boneABindRotationAbsolute = skeletonA.Skeleton.GetBindPoseAbsoluteInverse(boneIndexA).Rotation;
var boneBBindRotationAbsolute = skeletonB.Skeleton.GetBindPoseAbsoluteInverse(boneIndexB).Rotation.Inverse;
var relativeRotation = boneAActualRotationAbsolute * boneABindRotationAbsolute;
// Rotation: Using similarity transformation: (Read from right to left.)
// Rotate from model B space to model A space.
// Apply the bone transform rotation in model A space
// Rotate back from model A space to model B space.
relativeRotation = rotationAToB * relativeRotation * rotationBToA;
skeletonB.SetBoneRotationAbsolute(boneIndexB, relativeRotation * boneBBindRotationAbsolute);
// TODO: Map translations.
// How?
// Map translation relative to model space?
// Map translation relative to the next common bone ancestor that is in both skeletons
// (then we would need a mechanism or user input that gives us this ancestor, complicated)?
// Map translation relative to local space (the bone transform translation as in MapLocal)?
}
public static float Norm(QuaternionF q) => q.Norm;
/// <summary>
/// Perform mapping in local bone space.
/// </summary>
private static void MapLocal(bool mapTranslations, SkeletonPose skeletonA, int boneIndexA, SkeletonPose skeletonB, int boneIndexB, float scaleAToB, QuaternionF rotationBToA, QuaternionF rotationAToB)
{
var boneTransform = skeletonA.GetBoneTransform(boneIndexA);
// Remove any scaling.
boneTransform.Scale = Vector3F.One;
// Rotation: Using similarity transformation: (Read from right to left.)
// Rotate from bone B space to bone A space.
// Apply the bone transform rotation in bone A space
// Rotate back from bone A space to bone B space.
boneTransform.Rotation = rotationAToB * boneTransform.Rotation * rotationBToA;
// If we should map translations, then we scale the translation and rotate it from
// bone A space to bone B space.
if (mapTranslations)
boneTransform.Translation = rotationAToB.Rotate(boneTransform.Translation * scaleAToB);
else
boneTransform.Translation = Vector3F.Zero;
// Apply new bone transform to B.
skeletonB.SetBoneTransform(boneIndexB, boneTransform);
}
public static bool ApproximateEquals(this QuaternionF q0, QuaternionF q1, float tolerance)
{
return(ApproximateEquals(q0.W, q1.W, tolerance) && ApproximateEquals(q0.X, q1.X, tolerance) && ApproximateEquals(q0.Y, q1.Y, tolerance) && ApproximateEquals(q0.Z, q1.Z, tolerance));
}
private void CacheDerivedData()
{
// Abort if cached data is still valid.
if (!_isDirty)
return;
_isDirty = false;
// Check bone indices.
var skeletonInstanceA = SkeletonMapper.SkeletonPoseA;
var skeletonInstanceB = SkeletonMapper.SkeletonPoseB;
var skeletonA = skeletonInstanceA.Skeleton;
var skeletonB = skeletonInstanceB.Skeleton;
if (BoneIndexA < 0 || BoneIndexA >= skeletonInstanceA.Skeleton.NumberOfBones)
throw new IndexOutOfRangeException("BoneIndexA is out of range.");
if (BoneIndexB < 0 || BoneIndexB >= skeletonInstanceB.Skeleton.NumberOfBones)
throw new IndexOutOfRangeException("BoneIndexB is out of range.");
// Compute offsets.
var bindPoseAAbsoluteInverse = skeletonA.GetBindPoseAbsoluteInverse(BoneIndexA);
var bindPoseBAbsoluteInverse = skeletonB.GetBindPoseAbsoluteInverse(BoneIndexB);
if (MapAbsoluteTransforms)
{
_rotationAToB = SkeletonMapper.RotationOffset;
}
else
{
// Read from right to left:
// Change from A bone space to model A space.
// Apply rotation offset to change from model A space to model B space.
// Change model B space to B bone space.
// --> The result transforms from bone A space to bone B space.
_rotationAToB = bindPoseBAbsoluteInverse.Rotation * SkeletonMapper.RotationOffset * bindPoseAAbsoluteInverse.Rotation.Conjugated;
}
_rotationAToB.Normalize();
}
/// <summary>
/// Called when <see cref="IKSolver.Solve"/> is called.
/// </summary>
/// <param name="deltaTime">The current time step (in seconds).</param>
protected override void OnSolve(float deltaTime)
{
UpdateDerivedValues();
if (_totalChainLength == 0)
{
return;
}
var skeleton = SkeletonPose.Skeleton;
// Make a local copy of the absolute bone poses. The following operations will be performed
// on the data in _bone and not on the skeleton pose.
var numberOfBones = _boneIndices.Count;
for (int i = 0; i < numberOfBones; i++)
{
_bones[i] = SkeletonPose.GetBonePoseAbsolute(_boneIndices[i]);
}
// Calculate the position at the tip of the last bone.
// If TipOffset is not 0, then we can rotate the last bone.
// If TipOffset is 0, then the last bone defines the tip but is not rotated.
// --> numberOfBones is set to the number of affected bones.
Vector3F tipAbsolute;
if (TipOffset.IsNumericallyZero)
{
numberOfBones--;
tipAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex).Translation;
}
else
{
tipAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex).ToParentPosition(TipOffset);
}
// The root bone rotation that aligns the whole chain with the target.
QuaternionF chainRotation = QuaternionF.Identity;
Vector3F boneToTarget, boneToTip;
float remainingChainLength = _totalChainLength;
// Apply the soft limit to the distance to the IK goal
//vecToIkGoal = Target - _bones[0].Translation;
//distToIkGoal = vecToIkGoal.Length;
//// Limit the extension to 98% and ramp it up over 5% of the chains length
//vecToIkGoal *= (LimitValue(distToIkGoal, _totalChainLength * 0.98f, _totalChainLength * 0.08f)) / distToIkGoal;
//Vector3F goalPosition = _bones[0].Translation + vecToIkGoal;
var targetAbsolute = Target;
// This algorithms iterates once over all bones from root to tip.
for (int i = 0; i < numberOfBones; i++)
{
if (i > 0)
{
// Transform the bone position by the overall chain offset.
var translation = _bones[0].Translation
+ chainRotation.Rotate(_bones[i].Translation - _bones[0].Translation);
_bones[i] = new SrtTransform(_bones[i].Scale, _bones[i].Rotation, translation);
}
// The bone to tip vector of the aligned chain (without other IK rotations!).
boneToTip = tipAbsolute - _bones[i].Translation;
float boneToTipLength = boneToTip.Length;
boneToTip /= boneToTipLength; // TODO: Check for division by 0?
if (i > 0)
{
// Calculate the new absolute bone position.
var translation = _bones[i - 1].ToParentPosition(skeleton.GetBindPoseRelative(_boneIndices[i]).Translation);
_bones[i] = new SrtTransform(_bones[i].Scale, _bones[i].Rotation, translation);
}
// The bone to target vector of the new chain configuration.
boneToTarget = targetAbsolute - _bones[i].Translation;
float boneToTargetLength = boneToTarget.Length;
boneToTarget /= boneToTargetLength;
if (i == 0)
{
// This is the first bone: Compute rotation that aligns the whole initial chain with
// the target.
chainRotation = QuaternionF.CreateRotation(boneToTip, boneToTarget);
// Update tip.
tipAbsolute = _bones[i].Translation + (boneToTarget * boneToTipLength);
// Apply chainRotation to root bone.
_bones[i] = new SrtTransform(_bones[i].Scale, chainRotation * _bones[i].Rotation, _bones[i].Translation);
}
else
{
// Apply the chain alignment rotation. Also the parent bones have changed, so we apply
// an additional rotation that accounts for the ancestor rotations. This additional
// rotation aligns the last bone with the target.
// TODO: Find an explanation/derivation of this additional rotation.
_bones[i] = new SrtTransform(
_bones[i].Scale,
QuaternionF.CreateRotation(boneToTip, boneToTarget) * chainRotation * _bones[i].Rotation,
_bones[i].Translation);
}
// Now, solve the bone using trigonometry.
// The last bone was already aligned with the target. For the second last bone we use
// the law of cosines. For all other bones we use the complicated steps described in the
// GPG article.
if (i <= numberOfBones - 2)
{
// Length of chain after this bone.
remainingChainLength -= _boneLengths[i];
// The direction of the current bone. For the tip bone we use the TipOffset.
Vector3F boneDirection;
if (i != TipBoneIndex)
{
boneDirection = _bones[i].Rotation.Rotate(skeleton.GetBindPoseRelative(_boneIndices[i + 1]).Translation);
}
else
{
boneDirection = _bones[i].Rotation.Rotate(TipOffset);
}
if (!boneDirection.TryNormalize())
{
continue;
}
// The bone rotates around an axis normal to the bone to target direction and the bone
// vector.
Vector3F rotationAxis = Vector3F.Cross(boneToTarget, boneDirection);
if (!rotationAxis.TryNormalize())
{
continue; // TODO: If this happens, can we choose a useful direction?
}
// The current angle between bone direction and bone to target vector.
float currentAngle = (float)Math.Acos(MathHelper.Clamp(Vector3F.Dot(boneDirection, boneToTarget), -1, 1));
// Side lengths of the involved triangles.
var a = _boneLengths[i];
var b = boneToTargetLength;
var c = remainingChainLength;
var d = boneToTipLength;
float desiredAngle;
if (i == numberOfBones - 2)
{
// Use trigonometry (law of cosines) to determine the desired angle.
desiredAngle = (float)Math.Acos(MathHelper.Clamp((a * a + b * b - c * c) / (2 * a * b), -1.0f, 1.0f));
}
else
{
// The maximal angle that this bone can have where the chain still reaches the tip.
float maxTipAngle;
if (boneToTipLength > remainingChainLength)
{
maxTipAngle = (float)Math.Acos(MathHelper.Clamp((a * a + d * d - c * c) / (2 * a * d), -1.0f, 1.0f));
}
else
{
// Tip is very near and this bone can bend more than 180°. Add additional chain length
// in radians.
maxTipAngle = (float)Math.Acos(MathHelper.Clamp((a * 0.5f) / remainingChainLength, 0.0f, 1.0f));
maxTipAngle += ((c - d) / a);
}
// The maximal angle that this bone can have where the chain still reaches the target.
float maxTargetAngle;
if (boneToTargetLength > remainingChainLength)
{
maxTargetAngle = (float)Math.Acos(MathHelper.Clamp((a * a + b * b - c * c) / (2 * a * b), -1.0f, 1.0f));
}
else
{
// Target is very near and this bone can bend more than 180°. Add additional chain
// length in radians.
maxTargetAngle = (float)Math.Acos(MathHelper.Clamp((a * 0.5f) / remainingChainLength, 0.0f, 1.0f));
maxTargetAngle += ((c - b) / a);
}
// If we set the desired angle to maxTargetAngle, the remain bones must be all
// stretched. We want to keep the chain appearance, therefore, we set a smaller angle.
// The new angle relative to the final remaining chain should have the same ratio as the
// current angle to the current remaining chain.
if (!Numeric.IsZero(maxTipAngle))
{
desiredAngle = maxTargetAngle * (currentAngle / maxTipAngle);
}
else
{
desiredAngle = maxTargetAngle; // Avoiding divide by zero.
}
}
// The rotation angle that we have to apply.
float deltaAngle = desiredAngle - currentAngle;
// Apply the rotation to the current bones
_bones[i] = new SrtTransform(
_bones[i].Scale,
(QuaternionF.CreateRotation(rotationAxis, deltaAngle) * _bones[i].Rotation).Normalized,
_bones[i].Translation);
}
}
bool requiresBlending = RequiresBlending();
float maxRotationAngle;
bool requiresLimiting = RequiresLimiting(deltaTime, out maxRotationAngle);
if (requiresBlending || requiresLimiting)
{
// We have to blend the computed results with the original bone transforms.
// Get original bone transforms.
for (int i = 0; i < numberOfBones; i++)
{
_originalBoneTransforms.Add(SkeletonPose.GetBoneTransform(_boneIndices[i]));
}
for (int i = 0; i < numberOfBones; i++)
{
int boneIndex = _boneIndices[i];
var originalBoneTransform = _originalBoneTransforms[i];
// Set absolute bone pose and let the skeleton compute the bone transform for us.
SkeletonPose.SetBoneRotationAbsolute(boneIndex, _bones[i].Rotation);
var targetBoneTransform = SkeletonPose.GetBoneTransform(boneIndex);
// Apply weight.
if (requiresBlending)
{
BlendBoneTransform(ref originalBoneTransform, ref targetBoneTransform);
}
// Apply angular velocity limit.
if (requiresLimiting)
{
LimitBoneTransform(ref originalBoneTransform, ref targetBoneTransform, maxRotationAngle);
}
// Set final bone transform.
SkeletonPose.SetBoneTransform(boneIndex, targetBoneTransform);
}
_originalBoneTransforms.Clear();
}
else
{
// Weight is 1 and angular velocity limit is not active.
// --> Just copy the compute rotations.
for (int i = 0; i < numberOfBones; i++)
{
int boneIndex = _boneIndices[i];
SkeletonPose.SetBoneRotationAbsolute(boneIndex, _bones[i].Rotation);
}
}
}
protected static double CalculatePitch(QuaternionF orientation)
{
return(System.Math.Asin(2.0f * (orientation.W * orientation.Y - orientation.Z * orientation.X)));
}
/// <summary>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
// Estimate required segment angle for given accuracy.
// (Easy to derive from simple drawing of a circle segment with a triangle used to represent
// the segment.)
float alpha = (float)Math.Acos((_radius - absoluteDistanceThreshold) / _radius) * 2;
int numberOfSegments = (int)Math.Ceiling(ConstantsF.PiOver2 / alpha) * 4;
// Apply the iteration limit - in case absoluteDistanceThreshold is 0.
// Lets say each iteration doubles the number of segments. This is an arbitrary interpretation
// of the "iteration limit".
numberOfSegments = Math.Min(numberOfSegments, 2 << iterationLimit);
alpha = ConstantsF.TwoPi / numberOfSegments;
TriangleMesh mesh = new TriangleMesh();
// The world space vertices are created by rotating "radius vectors" with this rotations.
QuaternionF rotationY = QuaternionF.CreateRotationY(alpha);
QuaternionF rotationZ = QuaternionF.CreateRotationZ(alpha);
// We use two nested loops: In each loop a "radius vector" is rotated further to get a
// new vertex.
Vector3F rLow = Vector3F.UnitX * _radius; // Radius vector for the lower vertex.
for (int i = 1; i <= numberOfSegments / 4; i++)
{
Vector3F rHigh = rotationZ.Rotate(rLow); // Radius vector for the higher vertex.
// In the inner loop we create lines and triangles between 4 vertices, which are created
// with the radius vectors rLow0, rLow1, rHigh0, rHigh1.
Vector3F rLow0 = rLow;
Vector3F rHigh0 = rHigh;
for (int j = 1; j <= numberOfSegments; j++)
{
Vector3F rLow1 = rotationY.Rotate(rLow0);
Vector3F rHigh1 = rotationY.Rotate(rHigh0);
// Two top hemisphere triangles
mesh.Add(new Triangle
{
Vertex0 = rLow0,
Vertex1 = rLow1,
Vertex2 = rHigh0,
}, false);
if (i < numberOfSegments / 4) // At the "northpole" only a triangle is needed. No quad.
{
mesh.Add(new Triangle
{
Vertex0 = rLow1,
Vertex1 = rHigh1,
Vertex2 = rHigh0,
}, false);
}
// Two bottom hemisphere triangles
mesh.Add(new Triangle
{
Vertex0 = -rLow0,
Vertex1 = -rHigh0,
Vertex2 = -rLow1,
}, false);
if (i < numberOfSegments / 4) // At the "southpole" only a triangle is needed. No quad.
{
mesh.Add(new Triangle
{
Vertex0 = -rLow1,
Vertex1 = -rHigh0,
Vertex2 = -rHigh1,
}, false);
}
rLow0 = rLow1;
rHigh0 = rHigh1;
}
rLow = rHigh;
}
mesh.WeldVertices();
return(mesh);
}
protected static double CalculateYaw(QuaternionF orientation)
{
return(System.Math.Atan2(2.0f * (orientation.W * orientation.Z + orientation.X * orientation.Y), 1.0f - 2.0f * (orientation.Y * orientation.Y + orientation.Z * orientation.Z)));
}
public void ComputeCollision()
{
CollisionObject a = new CollisionObject();
CompositeShape compo = new CompositeShape();
compo.Children.Add(new GeometricObject(new SphereShape(2), Pose.Identity));
compo.Children.Add(new GeometricObject(new SphereShape(1), new Pose(new Vector3F(0, 3, 0))));
a.GeometricObject = new GeometricObject(compo, Pose.Identity);
CollisionObject b = new CollisionObject(new GeometricObject
{
Shape = new PlaneShape(new Vector3F(0, 1, 0), 1),
Pose = new Pose(new Vector3F(0, -1, 0)),
});
ContactSet set;
CompositeShapeAlgorithm algo = new CompositeShapeAlgorithm(new CollisionDetection());
set = algo.GetClosestPoints(a, b);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(true, algo.HaveContact(b, a));
Assert.AreEqual(1, set.Count);
Assert.AreEqual(2, set[0].PenetrationDepth);
Assert.AreEqual(new Vector3F(0, -2, 0), set[0].PositionAWorld);
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 2, 3), a.GeometricObject.Pose.Orientation);
set = set.Swapped;
algo.UpdateContacts(set, 0);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(true, algo.HaveContact(b, a));
Assert.AreEqual(1, set.Count);
Assert.AreEqual(0, set[0].PenetrationDepth);
Assert.AreEqual(new Vector3F(0, 0, 3), set[0].PositionBWorld);
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
set = set.Swapped; // new order: (a, b)
algo.UpdateContacts(set, 0);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(2, set.Count);
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-2, 0, 0), set[0].PositionALocal));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-1, 3, 0), set[1].PositionALocal));
Assert.AreEqual(new Vector3F(0, -1, 0), set[0].Normal);
Assert.AreEqual(new Vector3F(0, -1, 0), set[1].Normal);
Assert.AreEqual(2, set[0].PenetrationDepth);
Assert.IsTrue(Numeric.AreEqual(1, set[1].PenetrationDepth));
algo.UpdateClosestPoints(set, 0);
Assert.AreEqual(1, set.Count);
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 2.1f, 0), a.GeometricObject.Pose.Orientation);
algo.UpdateContacts(set, 0);
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(0, set.Count);
algo.UpdateClosestPoints(set, 0);
Assert.AreEqual(1, set.Count);
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-2, 0, 0), set[0].PositionALocal));
Assert.AreEqual(new Vector3F(0, -1, 0), set[0].Normal);
}
public override void Update(GameTime gameTime)
{
var mousePosition = InputService.MousePosition;
var viewport = GraphicsService.GraphicsDevice.Viewport;
var cameraNode = GraphicsScreen.CameraNode;
// Update picking ray.
if (InputService.IsDown(Keys.LeftControl) || InputService.IsDown(Keys.RightControl))
{
// Pick using mouse cursor.
SampleFramework.IsMouseVisible = true;
GraphicsScreen.DrawReticle = false;
// Convert the mouse screen position on the near viewing plane into a
// world space position.
Vector3 rayStart = viewport.Unproject(
new Vector3(mousePosition.X, mousePosition.Y, 0),
cameraNode.Camera.Projection,
(Matrix)cameraNode.View,
Matrix.Identity);
// Convert the mouse screen position on the far viewing plane into a
// world space position.
Vector3 rayEnd = viewport.Unproject(
new Vector3(mousePosition.X, mousePosition.Y, 1),
cameraNode.Camera.Projection,
(Matrix)cameraNode.View,
Matrix.Identity);
// Update ray. The ray should start at the near viewing plane under the
// mouse cursor and shoot into viewing direction. Therefore we change the
// pose of the ray object such that the ray origin is at rayStart and the
// orientation rotates the ray from shooting into +x direction into a ray
// shooting in viewing direction (rayEnd - rayStart).
((GeometricObject)_ray.GeometricObject).Pose = new Pose(
(Vector3F)rayStart,
QuaternionF.CreateRotation(Vector3F.Forward, (Vector3F)(rayEnd - rayStart)));
}
else
{
// Pick using reticle.
SampleFramework.IsMouseVisible = false;
GraphicsScreen.DrawReticle = true;
((GeometricObject)_ray.GeometricObject).Pose = cameraNode.PoseWorld;
}
// Update collision domain. This computes new contact information.
_domain.Update(gameTime.ElapsedGameTime);
// Draw objects. Change the color if the ray hits the object.
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
if (_domain.HaveContact(_ray, _box))
{
debugRenderer.DrawObject(_box.GeometricObject, Color.Red, false, false);
}
else
{
debugRenderer.DrawObject(_box.GeometricObject, Color.White, false, false);
}
if (_domain.HaveContact(_ray, _sphere))
{
debugRenderer.DrawObject(_sphere.GeometricObject, Color.Red, false, false);
}
else
{
debugRenderer.DrawObject(_sphere.GeometricObject, Color.White, false, false);
}
if (_domain.HaveContact(_ray, _mesh))
{
debugRenderer.DrawObject(_mesh.GeometricObject, Color.Red, false, false);
}
else
{
debugRenderer.DrawObject(_mesh.GeometricObject, Color.White, false, false);
}
// For triangle meshes we also know which triangle was hit!
// Get the contact information for ray-mesh hits.
ContactSet rayMeshContactSet = _domain.ContactSets.GetContacts(_ray, _mesh);
// rayMeshContactSet is a collection of Contacts between ray and mesh.
// If rayMeshContactSet is null or it contains no Contacts, then we have no contact.
if (rayMeshContactSet != null && rayMeshContactSet.Count > 0)
{
// A contact set contains information for a pair of touching objects A and B.
// We know that the two objects are ray and mesh, but we do not know if A or B
// is the ray.
bool objectAIsRay = rayMeshContactSet.ObjectA == _ray;
// Get the contact.
Contact contact = rayMeshContactSet[0];
// Get the feature index of the mesh feature that was hit.
int featureIndex = objectAIsRay ? contact.FeatureB : contact.FeatureA;
// For triangle meshes the feature index is the index of the triangle that was hit.
// Get the triangle from the triangle mesh shape.
Triangle triangle = ((TriangleMeshShape)_mesh.GeometricObject.Shape).Mesh.GetTriangle(featureIndex);
debugRenderer.DrawShape(new TriangleShape(triangle), _mesh.GeometricObject.Pose, _mesh.GeometricObject.Scale, Color.Yellow, false, false);
}
}
// OnLoad() is called when the GameObject is added to the IGameObjectService.
protected override void OnLoad()
{
var content = _services.GetInstance <ContentManager>();
_skyboxNode = new SkyboxNode(content.Load <TextureCube>("Sky2"))
{
Color = new Vector3F(SkyExposure),
};
// The ambient light.
var ambientLight = new AmbientLight
{
Color = new Vector3F(0.9f, 0.9f, 1f),
HdrScale = 0.1f,
Intensity = 0.5f,
HemisphericAttenuation = 0.8f,
};
_ambientLightNode = new LightNode(ambientLight)
{
Name = "Ambient",
};
// The main directional light.
var sunlight = new DirectionalLight
{
Color = new Vector3F(1, 0.9607844f, 0.9078432f),
HdrScale = 0.4f,
DiffuseIntensity = 1,
SpecularIntensity = 1,
};
_sunlightNode = new LightNode(sunlight)
{
Name = "Sunlight",
Priority = 10, // This is the most important light.
PoseWorld = new Pose(QuaternionF.CreateRotationY(-1.0f) * QuaternionF.CreateRotationX(-1.0f)),
// This light uses Cascaded Shadow Mapping.
Shadow = new CascadedShadow
{
PreferredSize = 1024,
DepthBiasScale = new Vector4F(0.98f),
DepthBiasOffset = new Vector4F(0.0f),
FadeOutDistance = 55,
MaxDistance = 75,
MinLightDistance = 100,
ShadowFog = 0.0f,
JitterResolution = 3000,
SplitDistribution = 0.7f,
}
};
// Add a lens flare for the key light.
var lensFlare = new LensFlare(true)
{
QuerySize = 0.2f, Size = 0.2f, Name = "Sun Flare"
};
var lensFlareTexture = content.Load <Texture2D>("LensFlare/LensFlares");
var circleTexture = new PackedTexture("Circle", lensFlareTexture, new Vector2F(0, 0), new Vector2F(0.25f, 0.5f));
var glowTexture = new PackedTexture("Glow", lensFlareTexture, new Vector2F(0.25f, 0), new Vector2F(0.25f, 0.5f));
var ringTexture = new PackedTexture("Ring", lensFlareTexture, new Vector2F(0.5f, 0), new Vector2F(0.25f, 0.5f));
var haloTexture = new PackedTexture("Halo", lensFlareTexture, new Vector2F(0.75f, 0), new Vector2F(0.25f, 0.5f));
var sunTexture = new PackedTexture("Sun", lensFlareTexture, new Vector2F(0, 0.5f), new Vector2F(0.25f, 0.5f));
var streaksTexture = new PackedTexture("Streaks", lensFlareTexture, new Vector2F(0.25f, 0.5f), new Vector2F(0.25f, 0.5f));
var flareTexture = new PackedTexture("Flare", lensFlareTexture, new Vector2F(0.5f, 0.5f), new Vector2F(0.25f, 0.5f));
lensFlare.Elements.Add(new LensFlareElement(-0.2f, 0.55f, 0.0f, new Color(175, 175, 255, 20), new Vector2F(0.5f, 0.5f), circleTexture));
lensFlare.Elements.Add(new LensFlareElement(0.0f, 0.9f, 0.0f, new Color(255, 255, 255, 255), new Vector2F(0.5f, 0.5f), sunTexture));
lensFlare.Elements.Add(new LensFlareElement(0.0f, 1.8f, 0.0f, new Color(255, 255, 255, 128), new Vector2F(0.5f, 0.5f), streaksTexture));
lensFlare.Elements.Add(new LensFlareElement(0.0f, 2.6f, 0.0f, new Color(255, 255, 200, 64), new Vector2F(0.5f, 0.5f), glowTexture));
lensFlare.Elements.Add(new LensFlareElement(0.5f, 0.12f, 0.0f, new Color(60, 60, 180, 35), new Vector2F(0.5f, 0.5f), circleTexture));
lensFlare.Elements.Add(new LensFlareElement(0.55f, 0.46f, 0.0f, new Color(100, 100, 200, 60), new Vector2F(0.5f, 0.5f), circleTexture));
lensFlare.Elements.Add(new LensFlareElement(0.6f, 0.17f, 0.0f, new Color(120, 120, 220, 40), new Vector2F(0.5f, 0.5f), circleTexture));
lensFlare.Elements.Add(new LensFlareElement(0.85f, 0.2f, 0.0f, new Color(60, 60, 255, 100), new Vector2F(0.5f, 0.5f), ringTexture));
lensFlare.Elements.Add(new LensFlareElement(1.5f, 0.2f, 0.0f, new Color(255, 60, 60, 130), new Vector2F(0.5f, 0.5f), flareTexture));
lensFlare.Elements.Add(new LensFlareElement(0.15f, 0.15f, 0.0f, new Color(255, 60, 60, 90), new Vector2F(0.5f, 0.5f), flareTexture));
lensFlare.Elements.Add(new LensFlareElement(1.3f, 0.6f, 0.0f, new Color(60, 60, 255, 180), new Vector2F(0.5f, 0.5f), haloTexture));
lensFlare.Elements.Add(new LensFlareElement(1.4f, 0.2f, 0.0f, new Color(220, 80, 80, 98), new Vector2F(0.5f, 0.5f), haloTexture));
lensFlare.Elements.Add(new LensFlareElement(1.5f, 0.1f, 0.0f, new Color(220, 80, 80, 85), new Vector2F(0.5f, 0.5f), circleTexture));
lensFlare.Elements.Add(new LensFlareElement(1.6f, 0.5f, 0.0f, new Color(60, 60, 255, 80), new Vector2F(0.5f, 0.5f), haloTexture));
lensFlare.Elements.Add(new LensFlareElement(1.8f, 0.3f, 0.0f, new Color(90, 60, 255, 110), new Vector2F(0.5f, 0.5f), ringTexture));
lensFlare.Elements.Add(new LensFlareElement(1.95f, 0.5f, 0.0f, new Color(60, 60, 255, 120), new Vector2F(0.5f, 0.5f), haloTexture));
lensFlare.Elements.Add(new LensFlareElement(2.0f, 0.15f, 0.0f, new Color(60, 60, 255, 85), new Vector2F(0.5f, 0.5f), circleTexture));
// Add lens flare as a child of the sunlight.
var lensFlareNode = new LensFlareNode(lensFlare);
_sunlightNode.Children = new SceneNodeCollection();
_sunlightNode.Children.Add(lensFlareNode);
// Add scene nodes to scene graph.
var scene = _services.GetInstance <IScene>();
scene.Children.Add(_skyboxNode);
scene.Children.Add(_ambientLightNode);
scene.Children.Add(_sunlightNode);
}
private bool _cullingEnabled = true; // True to use frustum culling. False to disable frustum culling.
public FrustumCullingSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
GraphicsScreen.ClearBackground = true;
GraphicsScreen.BackgroundColor = Color.CornflowerBlue;
// The top-down camera.
var orthographicProjection = new OrthographicProjection();
orthographicProjection.Set(
LevelSize * 1.1f * GraphicsService.GraphicsDevice.Viewport.AspectRatio,
LevelSize * 1.1f,
1,
10000f);
var topDownCamera = new Camera(orthographicProjection);
_topDownCameraNode = new CameraNode(topDownCamera)
{
View = Matrix44F.CreateLookAt(new Vector3F(0, 1000, 0), new Vector3F(0, 0, 0), -Vector3F.UnitZ),
};
// The perspective camera moving through the scene.
var perspectiveProjection = new PerspectiveProjection();
perspectiveProjection.SetFieldOfView(
MathHelper.ToRadians(45),
GraphicsService.GraphicsDevice.Viewport.AspectRatio,
1,
500);
var sceneCamera = new Camera(perspectiveProjection);
_sceneCameraNode = new CameraNode(sceneCamera);
// Initialize collision detection.
// We use one collision domain that manages all objects.
_domain = new CollisionDomain(new CollisionDetection())
{
// We exchange the default broad phase with a DualPartition. The DualPartition
// has special support for frustum culling.
BroadPhase = new DualPartition <CollisionObject>(),
};
// Create a lot of random objects and add them to the collision domain.
RandomHelper.Random = new Random(12345);
for (int i = 0; i < NumberOfObjects; i++)
{
// A real scene consists of a lot of complex objects such as characters, vehicles,
// buildings, lights, etc. When doing frustum culling we need to test each objects against
// the viewing frustum. If it intersects with the viewing frustum, the object is visible
// from the camera's point of view. However, in practice we do not test the exact object
// against the viewing frustum. Each objects is approximated by a simpler shape. In our
// example, we assume that each object is approximated with an oriented bounding box.
// (We could also use an other shape, such as a bounding sphere.)
// Create a random box.
Shape randomShape = new BoxShape(RandomHelper.Random.NextVector3F(1, 10));
// Create a random position.
Vector3F randomPosition;
randomPosition.X = RandomHelper.Random.NextFloat(-LevelSize / 2, LevelSize / 2);
randomPosition.Y = RandomHelper.Random.NextFloat(0, 2);
randomPosition.Z = RandomHelper.Random.NextFloat(-LevelSize / 2, LevelSize / 2);
// Create a random orientation.
QuaternionF randomOrientation = RandomHelper.Random.NextQuaternionF();
// Create object and add it to collision domain.
var geometricObject = new GeometricObject(randomShape, new Pose(randomPosition, randomOrientation));
var collisionObject = new CollisionObject(geometricObject)
{
CollisionGroup = 0,
};
_domain.CollisionObjects.Add(collisionObject);
}
// Per default, the collision domain computes collision between all objects.
// In this sample we do not need this information and disable it with a collision
// filter.
// In a real application, we would use this collision information for rendering,
// for example, to find out which lights overlap with which meshes, etc.
var filter = new CollisionFilter();
// Disable collision between objects in collision group 0.
filter.Set(0, 0, false);
_domain.CollisionDetection.CollisionFilter = filter;
// Start with the scene camera.
GraphicsScreen.CameraNode = _sceneCameraNode;
// We will collect a few statistics for debugging.
Profiler.SetFormat("NoCull", 1000, "Time in ms to submit DebugRenderer draw jobs without frustum culling.");
Profiler.SetFormat("WithCull", 1000, "Time in ms to submit DebugRenderer draw jobs with frustum culling.");
}
/// <summary>
/// Sets the bone rotation of a bone so that it matches the given rotation in model space.
/// </summary>
/// <param name="skeletonPose">The skeleton pose.</param>
/// <param name="boneIndex">The index of the bone.</param>
/// <param name="rotation">The rotation in model space.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="skeletonPose" /> is <see langword="null"/>.
/// </exception>
public static void SetBoneRotationAbsolute(this SkeletonPose skeletonPose, int boneIndex, QuaternionF rotation)
{
if (skeletonPose == null)
{
throw new ArgumentNullException("skeletonPose");
}
var skeleton = skeletonPose.Skeleton;
var bindPoseRelative = skeleton.GetBindPoseRelative(boneIndex);
var parentIndex = skeleton.GetParent(boneIndex);
var parentBonePoseAbsolute = (parentIndex >= 0) ? skeletonPose.GetBonePoseAbsolute(parentIndex) : SrtTransform.Identity;
// Solving this equation (using only rotations):
// rotation = parentBonePoseAbsolute * bindPoseRelative * rotationRelative;
// rotationRelative = boneTransform.
var rotationRelative = bindPoseRelative.Rotation.Conjugated
* parentBonePoseAbsolute.Rotation.Conjugated
* rotation;
rotationRelative.Normalize();
var boneTransform = skeletonPose.GetBoneTransform(boneIndex);
boneTransform.Rotation = rotationRelative;
skeletonPose.SetBoneTransform(boneIndex, boneTransform);
}
public void Interpolate()
{
Pose p1 = new Pose(new Vector3F(1, 2, 3), QuaternionF.CreateRotationY(0.3f));
Pose p2 = new Pose(new Vector3F(-4, 5, -6), QuaternionF.CreateRotationZ(-0.1f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(p1.Position, Pose.Interpolate(p1, p2, 0).Position));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(p1.Orientation, Pose.Interpolate(p1, p2, 0).Orientation));
Assert.IsTrue(Vector3F.AreNumericallyEqual(p2.Position, Pose.Interpolate(p1, p2, 1).Position));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(p2.Orientation, Pose.Interpolate(p1, p2, 1).Orientation));
Assert.IsTrue(Vector3F.AreNumericallyEqual(InterpolationHelper.Lerp(p1.Position, p2.Position, 0.3f), Pose.Interpolate(p1, p2, 0.3f).Position));
Assert.IsTrue(
QuaternionF.AreNumericallyEqual(
InterpolationHelper.Lerp(QuaternionF.CreateRotation(p1.Orientation), QuaternionF.CreateRotation(p2.Orientation), 0.3f),
QuaternionF.CreateRotation(Pose.Interpolate(p1, p2, 0.3f).Orientation)));
}
protected static double CalculatePitch(QuaternionF orientation)
{
//Contract.Requires<ArgumentNullException>(orientation != null, "orientation");
return(System.Math.Asin(2.0f * (orientation.W * orientation.Y - orientation.Z * orientation.X)));
}
public static QuaternionF Inverse(QuaternionF q) => q.Inverse;
//public static void RotateBoneWorld(this SkeletonPose SkeletonPose, int boneIndex, QuaternionF rotation, Matrix44F world)
//{
// QuaternionF worldRotation = QuaternionF.CreateRotation(world.Minor);
// RotateBoneAbsolute(SkeletonPose, boneIndex, worldRotation.Conjugated * rotation);
//}
// TODO: This method should really be called RotateBoneLocalAnimated?
///// <summary>
///// Rotates bone where the rotation is given in the bone space.
///// </summary>
///// <param name="skeletonPose">The skeleton pose.</param>
///// <param name="boneIndex">The index of the bone.</param>
///// <param name="rotation">The rotation in bone space.</param>
///// <exception cref="ArgumentNullException">
///// <paramref name="skeletonPose" /> is <see langword="null"/>.
///// </exception>
//public static void RotateBoneLocal(this SkeletonPose skeletonPose, int boneIndex, QuaternionF rotation)
//{
// if (skeletonPose == null)
// throw new ArgumentNullException("skeletonPose");
// var boneTransform = skeletonPose.GetBoneTransform(boneIndex);
// boneTransform.Rotation = boneTransform.Rotation * rotation;
// skeletonPose.SetBoneTransform(boneIndex, boneTransform);
//}
/// <summary>
/// Rotates a bone where the rotation is given in model space.
/// </summary>
/// <param name="skeletonPose">The skeleton pose.</param>
/// <param name="boneIndex">The index of the bone.</param>
/// <param name="rotation">The rotation in model space.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="skeletonPose" /> is <see langword="null"/>.
/// </exception>
public static void RotateBoneAbsolute(this SkeletonPose skeletonPose, int boneIndex, QuaternionF rotation)
{
if (skeletonPose == null)
throw new ArgumentNullException("skeletonPose");
var skeleton = skeletonPose.Skeleton;
var boneTransform = skeletonPose.GetBoneTransform(boneIndex);
var bindPoseRelative = skeleton.GetBindPoseRelative(boneIndex);
var parentIndex = skeleton.GetParent(boneIndex);
var parentBonePoseAbsolute = skeletonPose.GetBonePoseAbsolute(parentIndex);
// Solving these equations (using only the rotations):
// rotation * bonePoseAbsolute = bonePoseAbsoluteNew
// bonePoseAbsolute = parentBonePoseAbsolute * bindPoseRelative * boneTransform.
// ...
// Rotation relative to bone bind pose space (using similarity transformation).
var rotationRelative = bindPoseRelative.Rotation.Conjugated
* parentBonePoseAbsolute.Rotation.Conjugated
* rotation
* parentBonePoseAbsolute.Rotation
* bindPoseRelative.Rotation;
// The final rotation is: First rotate into bone bind pose space, then apply rotation.
boneTransform.Rotation = rotationRelative * boneTransform.Rotation;
// So many multiplications, numerical errors adds up quickly in iterative IK algorithms...
boneTransform.Rotation.Normalize();
skeletonPose.SetBoneTransform(boneIndex, boneTransform);
}
public static QuaternionF Normalized(QuaternionF q) => q.Normalized;
private void CreateRoad()
{
//RandomHelper.Random = new Random(1234567);
// Set isClosed to true join the start and the end of the road.
bool isClosed = false;
// Create a new TerrainRoadLayer which paints a road onto the terrain.
// The road itself is defined by a mesh which is set later.
_roadLayer = new TerrainRoadLayer(GraphicsService)
{
DiffuseColor = new Vector3F(0.5f),
SpecularColor = new Vector3F(1),
DiffuseTexture = ContentManager.Load <Texture2D>("Terrain/Road-Asphalt-Diffuse"),
NormalTexture = ContentManager.Load <Texture2D>("Terrain/Road-Asphalt-Normal"),
SpecularTexture = ContentManager.Load <Texture2D>("Terrain/Road-Asphalt-Specular"),
HeightTexture = ContentManager.Load <Texture2D>("Terrain/Road-Asphalt-Height"),
// The size of the tileable detail textures in world space units.
TileSize = 5,
// The border blend range controls how the border of the road fades out.
// We fade out 5% of the texture on each side of the road.
BorderBlendRange = new Vector4F(0.05f, 0.05f, 0.05f, 0.05f),
};
// Create 3D spline path with some semi-random control points.
_roadPath = new Path3F
{
PreLoop = isClosed ? CurveLoopType.Cycle : CurveLoopType.Linear,
PostLoop = isClosed ? CurveLoopType.Cycle : CurveLoopType.Linear,
SmoothEnds = isClosed,
};
// The position of the next path key.
Vector3F position = new Vector3F(
RandomHelper.Random.NextFloat(-20, 20),
0,
RandomHelper.Random.NextFloat(-20, 20));
// The direction to the next path key.
Vector3F direction = QuaternionF.CreateRotationY(RandomHelper.Random.NextFloat(0, 10)).Rotate(Vector3F.Forward);
// Add path keys.
for (int j = 0; j < 10; j++)
{
// Instead of a normal PathKey3F, we use a TerrainRoadPathKey which allows to control
// the road with and the side falloff.
var key = new TerrainRoadPathKey
{
Interpolation = SplineInterpolation.CatmullRom,
Parameter = j,
Point = position,
// The width of the road at the path key.
Width = RandomHelper.Random.NextFloat(6, 10),
// The side falloff (which is used in ClampTerrainToRoad to blend the height values with
// the road).
SideFalloff = RandomHelper.Random.NextFloat(20, 40),
};
_roadPath.Add(key);
// Get next random position and direction.
position += direction * RandomHelper.Random.NextFloat(20, 40);
position.Y += RandomHelper.Random.NextFloat(-2, 2);
direction = QuaternionF.CreateRotationY(RandomHelper.Random.NextFloat(-1, 1))
.Rotate(direction);
}
if (isClosed)
{
// To create a closed path the first and the last key should be identical.
_roadPath[_roadPath.Count - 1].Point = _roadPath[0].Point;
((TerrainRoadPathKey)_roadPath[_roadPath.Count - 1]).Width = ((TerrainRoadPathKey)_roadPath[0]).Width;
((TerrainRoadPathKey)_roadPath[_roadPath.Count - 1]).SideFalloff =
((TerrainRoadPathKey)_roadPath[0]).SideFalloff;
// Since the path is closed we do not have to fade out the start and the end of the road.
_roadLayer.BorderBlendRange *= new Vector4F(1, 0, 1, 0);
}
// Convert the path to a mesh.
Submesh roadSubmesh;
Aabb roadAabb;
float roadLength;
TerrainRoadLayer.CreateMesh(
GraphicsService.GraphicsDevice,
_roadPath,
4,
10,
0.1f,
out roadSubmesh,
out roadAabb,
out roadLength);
// Set the mesh in the road layer.
_roadLayer.SetMesh(roadSubmesh, roadAabb, roadLength, true);
if (isClosed)
{
// When the path is closed, the end texture and the start texture coordinates should
// match. This is the case if (roadLength / tileSize) is an integer.
var numberOfTiles = (int)(roadLength / _roadLayer.TileSize);
_roadLayer.TileSize = roadLength / numberOfTiles;
}
// The road layer is added to the layers of a tile. We have to add the road to each terrain
// tile which it overlaps.
foreach (var tile in _terrainObject.TerrainNode.Terrain.Tiles)
{
if (GeometryHelper.HaveContact(tile.Aabb, _roadLayer.Aabb.Value))
{
tile.Layers.Add(_roadLayer);
}
}
}
protected override void OnUpdate(TimeSpan deltaTime)
{
MousePicking();
if (_inputService.IsDown(Keys.W))
{
_position -= (deltaTime.Milliseconds / 10.0f) * new Vector3F(GetForwardVector().X, 0.0f, GetForwardVector().Z);
}
if (_inputService.IsDown(Keys.A))
{
_position -= (deltaTime.Milliseconds / 10.0f) * GetRightVector();
}
if (_inputService.IsDown(Keys.S))
{
_position += (deltaTime.Milliseconds / 10.0f) * new Vector3F(GetForwardVector().X, 0.0f, GetForwardVector().Z);
}
if (_inputService.IsDown(Keys.D))
{
_position += (deltaTime.Milliseconds / 10.0f) * GetRightVector();
}
if (_inputService.IsDown(Keys.E))
{
_pitch -= Microsoft.Xna.Framework.MathHelper.ToRadians(deltaTime.Milliseconds / 20.0f);
}
if (_inputService.IsDown(Keys.Q))
{
_pitch += Microsoft.Xna.Framework.MathHelper.ToRadians(deltaTime.Milliseconds / 20.0f);
}
_position -= new Vector3F(0.0f, (_inputService.MouseWheelDelta / 20.0f) * GetUpVector().Y, 0.0f);
_orientation = QuaternionF.CreateRotationY(_pitch) * QuaternionF.CreateRotationX(_yaw);
_position.X = Microsoft.Xna.Framework.MathHelper.Clamp(_position.X, -250.0f, 250.0f);
_position.Y = Microsoft.Xna.Framework.MathHelper.Clamp(_position.Y, 60.0f, 270.0f);
_position.Z = Microsoft.Xna.Framework.MathHelper.Clamp(_position.Z, -250.0f, 250.0f);
_cameraNode.PoseWorld = new Pose(_position, _orientation);
View = _cameraNode.View.ToXna();
Projection = _cameraNode.Camera.Projection.ToXna();
base.OnUpdate(deltaTime);
}
public void GetEventOrientation_ValidParameters_ExpectedBridgeCalls()
{
// Setup
var myoHandle = new IntPtr(123);
var eventHandle = new IntPtr(789);
var expectedResult = new QuaternionF(10, 20, 30, 40);
var bridge = new Mock <IMyoDeviceBridge>(MockBehavior.Strict);
bridge
.Setup(x => x.EventGetOrientation32(eventHandle, OrientationIndex.W))
.Returns(expectedResult.W);
bridge
.Setup(x => x.EventGetOrientation32(eventHandle, OrientationIndex.X))
.Returns(expectedResult.X);
bridge
.Setup(x => x.EventGetOrientation32(eventHandle, OrientationIndex.Y))
.Returns(expectedResult.Y);
bridge
.Setup(x => x.EventGetOrientation32(eventHandle, OrientationIndex.Z))
.Returns(expectedResult.Z);
bridge
.Setup(x => x.EventGetOrientation64(eventHandle, OrientationIndex.W))
.Returns(expectedResult.W);
bridge
.Setup(x => x.EventGetOrientation64(eventHandle, OrientationIndex.X))
.Returns(expectedResult.X);
bridge
.Setup(x => x.EventGetOrientation64(eventHandle, OrientationIndex.Y))
.Returns(expectedResult.Y);
bridge
.Setup(x => x.EventGetOrientation64(eventHandle, OrientationIndex.Z))
.Returns(expectedResult.Z);
var myoErrorHandlerDriver = new Mock <IMyoErrorHandlerDriver>(MockBehavior.Strict);
var driver = MyoDeviceDriver.Create(
myoHandle,
bridge.Object,
myoErrorHandlerDriver.Object);
// Execute
var result = driver.GetEventOrientation(eventHandle);
// Assert
Assert.Equal(expectedResult.W, result.W);
Assert.Equal(expectedResult.X, result.X);
Assert.Equal(expectedResult.Y, result.Y);
Assert.Equal(expectedResult.Z, result.Z);
bridge.Verify(x => x.EventGetOrientation32(eventHandle, OrientationIndex.W), PlatformInvocation.Running32Bit ? Times.Once() : Times.Never());
bridge.Verify(x => x.EventGetOrientation32(eventHandle, OrientationIndex.X), PlatformInvocation.Running32Bit ? Times.Once() : Times.Never());
bridge.Verify(x => x.EventGetOrientation32(eventHandle, OrientationIndex.Y), PlatformInvocation.Running32Bit ? Times.Once() : Times.Never());
bridge.Verify(x => x.EventGetOrientation32(eventHandle, OrientationIndex.Z), PlatformInvocation.Running32Bit ? Times.Once() : Times.Never());
bridge.Verify(x => x.EventGetOrientation64(eventHandle, OrientationIndex.W), PlatformInvocation.Running32Bit ? Times.Never() : Times.Once());
bridge.Verify(x => x.EventGetOrientation64(eventHandle, OrientationIndex.X), PlatformInvocation.Running32Bit ? Times.Never() : Times.Once());
bridge.Verify(x => x.EventGetOrientation64(eventHandle, OrientationIndex.Y), PlatformInvocation.Running32Bit ? Times.Never() : Times.Once());
bridge.Verify(x => x.EventGetOrientation64(eventHandle, OrientationIndex.Z), PlatformInvocation.Running32Bit ? Times.Never() : Times.Once());
}
// Initializes the bone rotations using the quaternions of the specified array.
private void ArrayToSkeletonPose(float[] values)
{
var numberOfBones = SkeletonPose.Skeleton.NumberOfBones;
for (int i = 0; i < numberOfBones; i++)
{
QuaternionF quaternion = new QuaternionF(
values[i * 4 + 0],
values[i * 4 + 1],
values[i * 4 + 2],
values[i * 4 + 3]);
// The quaternions were filtered using component-wise linear interpolation. This
// is only an approximation which denormalizes the quaternions.
// --> Renormalize the quaternions.
quaternion.TryNormalize();
// Exchange the rotation in the bone transform.
var boneTransform = SkeletonPose.GetBoneTransform(i);
boneTransform.Rotation = quaternion;
SkeletonPose.SetBoneTransform(i, boneTransform);
}
}
public ShapesSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0))
{
Name = "GroundPlane", // Names are not required but helpful for debugging.
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(groundPlane);
// ----- Add a sphere.
Shape sphere = new SphereShape(0.5f);
Simulation.RigidBodies.Add(new RigidBody(sphere));
// ----- Add a box.
BoxShape box = new BoxShape(0.5f, 0.9f, 0.7f);
Simulation.RigidBodies.Add(new RigidBody(box));
// ----- Add a capsule.
CapsuleShape capsule = new CapsuleShape(0.4f, 1.2f);
Simulation.RigidBodies.Add(new RigidBody(capsule));
// ----- Add a cone.
ConeShape cone = new ConeShape(0.5f, 1f);
Simulation.RigidBodies.Add(new RigidBody(cone));
// ----- Add a cylinder.
CylinderShape cylinder = new CylinderShape(0.3f, 1f);
Simulation.RigidBodies.Add(new RigidBody(cylinder));
// ----- Add a convex hull of random points.
ConvexHullOfPoints convexHullOfPoints = new ConvexHullOfPoints();
for (int i = 0; i < 20; i++)
{
convexHullOfPoints.Points.Add(RandomHelper.Random.NextVector3F(-0.5f, 0.5f));
}
Simulation.RigidBodies.Add(new RigidBody(convexHullOfPoints));
// ----- Add a convex polyhedron.
// (A ConvexPolyhedron is similar to the ConvexHullOfPoints. The difference is that
// the points in a ConvexHullOfPoints can be changed at runtime. A ConvexPolyhedron
// cannot be changed at runtime, but it is faster.)
List <Vector3F> points = new List <Vector3F>();
for (int i = 0; i < 20; i++)
{
points.Add(RandomHelper.Random.NextVector3F(-0.7f, 0.7f));
}
ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(points);
Simulation.RigidBodies.Add(new RigidBody(convexPolyhedron));
// ----- Add a composite shape (a table that consists of 5 boxes).
CompositeShape composite = new CompositeShape();
composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(-0.75f, 0.4f, -0.5f))));
composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(0.75f, 0.4f, -0.5f))));
composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(-0.75f, 0.4f, 0.5f))));
composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(0.75f, 0.4f, 0.5f))));
composite.Children.Add(new GeometricObject(new BoxShape(1.8f, 0.1f, 1.1f), new Pose(new Vector3F(0, 0.8f, 0))));
Simulation.RigidBodies.Add(new RigidBody(composite));
// ----- Add a convex hull of multiple shapes.
ConvexHullOfShapes convexHullOfShapes = new ConvexHullOfShapes();
convexHullOfShapes.Children.Add(new GeometricObject(new CylinderShape(0.2f, 0.8f), new Pose(new Vector3F(-0.4f, 0, 0))));
convexHullOfShapes.Children.Add(new GeometricObject(new CylinderShape(0.2f, 0.8f), new Pose(new Vector3F(+0.4f, 0, 0))));
Simulation.RigidBodies.Add(new RigidBody(convexHullOfShapes));
// ----- Add the Minkowski sum of two shapes.
// (The Minkowski sum is a mathematical operation that combines two shapes.
// Here a circle is combined with a sphere. The result is a wheel.)
MinkowskiSumShape minkowskiSum = new MinkowskiSumShape();
minkowskiSum.ObjectA = new GeometricObject(new SphereShape(0.2f), Pose.Identity);
minkowskiSum.ObjectB = new GeometricObject(new CircleShape(0.5f), Pose.Identity);
Simulation.RigidBodies.Add(new RigidBody(minkowskiSum));
// Create another Minkowski sum. (Here a small sphere is added to a box to create a
// box with rounded corners.)
minkowskiSum = new MinkowskiSumShape();
minkowskiSum.ObjectA = new GeometricObject(new SphereShape(0.1f), Pose.Identity);
minkowskiSum.ObjectB = new GeometricObject(new BoxShape(0.2f, 0.5f, 0.8f), Pose.Identity);
Simulation.RigidBodies.Add(new RigidBody(minkowskiSum));
// ----- Add a triangle mesh.
// A triangle mesh could be loaded from a file or built from an XNA model.
// Here we first create a composite shape and convert the shape into a triangle
// mesh. (Any Shape in DigitalRune.Geometry can be converted to a triangle mesh.)
CompositeShape dumbbell = new CompositeShape();
dumbbell.Children.Add(new GeometricObject(new SphereShape(0.4f), new Pose(new Vector3F(0.6f, 0.0f, 0.0f))));
dumbbell.Children.Add(new GeometricObject(new SphereShape(0.4f), new Pose(new Vector3F(-0.6f, 0.0f, 0.0f))));
dumbbell.Children.Add(new GeometricObject(new CylinderShape(0.1f, 0.6f), new Pose(Matrix33F.CreateRotationZ(ConstantsF.PiOver2))));
TriangleMeshShape triangleMeshShape = new TriangleMeshShape(dumbbell.GetMesh(0.01f, 2));
// Optional: We can enable "contact welding". When this flag is enabled, the triangle shape
// precomputes additional internal information for the mesh. The collision detection will
// be able to compute better contacts (e.g. better normal vectors at triangle edges).
// Pro: Collision detection can compute better contact information.
// Con: Contact welding information needs a bit of memory. And the collision detection is
// a bit slower.
triangleMeshShape.EnableContactWelding = true;
// Optional: Assign a spatial partitioning scheme to the triangle mesh. (A spatial partition
// adds an additional memory overhead, but it improves collision detection speed tremendously!)
triangleMeshShape.Partition = new AabbTree <int>()
{
// The tree is automatically built using a mixed top-down/bottom-up approach. Bottom-up
// building is slower but produces better trees. If the tree building takes too long,
// we can lower the BottomUpBuildThreshold (default is 128).
BottomUpBuildThreshold = 0,
};
Simulation.RigidBodies.Add(new RigidBody(triangleMeshShape));
// ----- Set random positions/orientations.
// (Start with the second object. The first object is the ground plane which should
// not be changed.)
for (int i = 1; i < Simulation.RigidBodies.Count; i++)
{
RigidBody body = Simulation.RigidBodies[i];
Vector3F position = RandomHelper.Random.NextVector3F(-3, 3);
position.Y = 3; // Position the objects 3m above ground.
QuaternionF orientation = RandomHelper.Random.NextQuaternionF();
body.Pose = new Pose(position, orientation);
}
}
public void LengthSquared()
{
QuaternionF q = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f);
float lengthSquared = 1 + 4 + 9 + 16;
Assert.AreEqual(lengthSquared, q.Norm);
}
public static QuaternionF Conjugated(QuaternionF q) => q.Conjugated;
public void Ln4()
{
float θ = 0.0f;
Vector3F v = new Vector3F(1.0f, 2.0f, 3.0f);
v.Normalize();
QuaternionF q = new QuaternionF((float)Math.Cos(θ), (float)Math.Sin(θ) * v);
q.Ln();
Assert.IsTrue(Numeric.AreEqual(0.0f, q.W));
Assert.IsTrue(Vector3F.AreNumericallyEqual(θ * v, q.V));
}
private static void Append(StringBuilder text, QuaternionF q)
{
text.Append(string.Format(CultureInfo.InvariantCulture, "new QuaternionF({0}f, {1}f, {2}f, {3}f)",
q.W, q.X, q.Y, q.Z));
}
public static float Dot(this QuaternionF a, QuaternionF b)
{
return(a.W * b.W + a.X * b.X + a.Y * b.Y + a.Z * b.Z);
}
private static Vector4 ToVector(QuaternionF quaternion)
{
return new Vector4(quaternion.X, quaternion.Y, quaternion.Z, quaternion.W);
}
public static float NormSquared(QuaternionF q) => q.NormSquared;
