public void GetAxisAlignedBoundingBox()
{
Assert.AreEqual(new Aabb(), new BoxShape().GetAabb(Pose.Identity));
Assert.AreEqual(new Aabb(new Vector3F(10, 100, -13), new Vector3F(10, 100, -13)),
new BoxShape().GetAabb(new Pose(new Vector3F(10, 100, -13),
QuaternionF.CreateRotation(new Vector3F(1, 1, 1), 0.7f))));
Assert.AreEqual(new Aabb(new Vector3F(5, 90, 985), new Vector3F(15, 110, 1015)),
new BoxShape(10, 20, 30).GetAabb(new Pose(new Vector3F(10, 100, 1000),
QuaternionF.Identity)));
// TODO: Test rotations.
}
public void GetAxisAlignedBoundingBox()
{
Assert.AreEqual(new Aabb(), new CapsuleShape().GetAabb(Pose.Identity));
Assert.AreEqual(new Aabb(new Vector3F(10, 100, -13), new Vector3F(10, 100, -13)),
new CapsuleShape().GetAabb(new Pose(new Vector3F(10, 100, -13),
QuaternionF.CreateRotation(new Vector3F(1, 1, 1), 0.7f))));
Assert.AreEqual(new Aabb(new Vector3F(0, 80, 990), new Vector3F(20, 120, 1010)),
new CapsuleShape(10, 40).GetAabb(new Pose(new Vector3F(10, 100, 1000),
QuaternionF.Identity)));
// TODO: Test rotations.
}
// Add light sources for standard three-point lighting.
private static void AddLights(Scene scene)
{
var ambientLight = new AmbientLight
{
Color = new Vector3F(0.05333332f, 0.09882354f, 0.1819608f),
Intensity = 1,
HemisphericAttenuation = 0,
};
scene.Children.Add(new LightNode(ambientLight));
var keyLight = new DirectionalLight
{
Color = new Vector3F(1, 0.9607844f, 0.8078432f),
DiffuseIntensity = 1,
SpecularIntensity = 1,
};
var keyLightNode = new LightNode(keyLight)
{
Name = "KeyLight",
Priority = 10, // This is the most important light.
PoseWorld = new Pose(QuaternionF.CreateRotation(Vector3F.Forward, new Vector3F(-0.5265408f, -0.5735765f, -0.6275069f))),
};
scene.Children.Add(keyLightNode);
var fillLight = new DirectionalLight
{
Color = new Vector3F(0.9647059f, 0.7607844f, 0.4078432f),
DiffuseIntensity = 1,
SpecularIntensity = 0,
};
var fillLightNode = new LightNode(fillLight)
{
Name = "FillLight",
PoseWorld = new Pose(QuaternionF.CreateRotation(Vector3F.Forward, new Vector3F(0.7198464f, 0.3420201f, 0.6040227f))),
};
scene.Children.Add(fillLightNode);
var backLight = new DirectionalLight
{
Color = new Vector3F(0.3231373f, 0.3607844f, 0.3937255f),
DiffuseIntensity = 1,
SpecularIntensity = 1,
};
var backLightNode = new LightNode(backLight)
{
Name = "BackLight",
PoseWorld = new Pose(QuaternionF.CreateRotation(Vector3F.Forward, new Vector3F(0.4545195f, -0.7660444f, 0.4545195f))),
};
scene.Children.Add(backLightNode);
}
private static void Append(StringBuilder text, CollisionObject co, bool isFirstCollisionObject)
{
text.Append(@"
{
var mesh = new TriangleMesh();");
var shape = co.GeometricObject.Shape as TriangleMeshShape;
if (shape == null)
{
throw new NotSupportedException("The shapes must be TriangleMeshShapes");
}
var mesh = shape.Mesh;
for (int i = 0; i < mesh.NumberOfTriangles; i++)
{
text.Append(@"
mesh.Add(");
Append(text, mesh.GetTriangle(i));
text.Append(");");
}
text.Append(@"
var pose = new Pose(");
Append(text, co.GeometricObject.Pose.Position);
text.Append(", ");
Append(text, QuaternionF.CreateRotation(co.GeometricObject.Pose.Orientation));
text.Append(@");
var scale = ");
Append(text, co.GeometricObject.Scale);
text.Append(@";
var shape = new TriangleMeshShape(mesh) { Partition = new CompressedAabbTree() };
shape.IsTwoSided = ");
Append(text, shape.IsTwoSided);
text.Append(@";
shape.EnableContactWelding = ");
Append(text, shape.EnableContactWelding);
text.Append(@";
");
if (isFirstCollisionObject)
{
text.Append("_objectA");
}
else
{
text.Append("_objectB");
}
text.Append(@" = new CollisionObject(new GeometricObject(shape, scale, pose));
}");
}
public void Angle()
{
Vector3F axis = new Vector3F(1.0f, 2.0f, 3.0f);
QuaternionF q = QuaternionF.CreateRotation(axis, 0.4f);
Assert.IsTrue(Numeric.AreEqual(0.4f, q.Angle));
q.Angle = 0.9f;
Assert.IsTrue(QuaternionF.AreNumericallyEqual(q, QuaternionF.CreateRotation(axis, 0.9f)));
Assert.AreEqual(0, new QuaternionF(1.000001f, 0, 0, 0).Angle);
}
public void SlerpNegatedSinglePrecision()
{
QuaternionF q1 = new QuaternionF(-1.0f, 0.0f, 0.0f, 0.0f);
QuaternionF q2 = QuaternionF.CreateRotation(-Vector3F.UnitZ, (float)-Math.PI / 2);
QuaternionF slerp = InterpolationHelper.Slerp(q1, q2, 0.5f);
Assert.IsTrue(slerp.IsNumericallyNormalized);
Vector3F v = slerp.Rotate(Vector3F.UnitX);
Vector3F result = new Vector3F(1.0f, 1.0f, 0.0f).Normalized;
Assert.IsTrue(Vector3F.AreNumericallyEqual(result, v));
}
public void RotationMatrix33()
{
float angle = -1.6f;
Vector3F axis = new Vector3F(1.0f, 2.0f, -3.0f);
QuaternionF q = QuaternionF.CreateRotation(axis, angle);
Matrix33F m33 = Matrix33F.CreateRotation(axis, angle);
Vector3F v = new Vector3F(0.3f, -2.4f, 5.6f);
Vector3F result1 = q.ToRotationMatrix33() * v;
Vector3F result2 = m33 * v;
Assert.IsTrue(Vector3F.AreNumericallyEqual(result1, result2));
}
private void OnMouseMove(object sender, MouseEventArgs eventArgs)
{
if (eventArgs.LeftButton == MouseButtonState.Pressed)
{
// Rotate view.
Point mousePosition = eventArgs.GetPosition(AssociatedObject);
Vector3F dragVector = MapToSphere(mousePosition);
Matrix44F rotation = QuaternionF.CreateRotation(_startVector, dragVector).ToRotationMatrix44();
_transform = rotation * _originalTransform;
UpdateCamera(_cameraNode);
}
}
public void CreateRotationZ()
{
float angle = (float)MathHelper.ToRadians(30);
Matrix33F m = Matrix33F.CreateRotationZ(angle);
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F((float)Math.Cos(angle), (float)Math.Sin(angle), 0), m * Vector3F.UnitX));
QuaternionF q = QuaternionF.CreateRotation(Vector3F.UnitZ, angle);
Assert.IsTrue(Vector3F.AreNumericallyEqual(q.Rotate(Vector3F.One), m * Vector3F.One));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(Matrix33F.CreateRotation(Vector3F.UnitZ, angle), m));
}
/// <summary>
/// Interpolates two poses.
/// </summary>
/// <param name="startPose">The start pose.</param>
/// <param name="endPose">The end pose.</param>
/// <param name="parameter">
/// The interpolation parameter. If the value is 0, the <paramref name="startPose"/> is
/// returned. If the value is 1, the <paramref name="endPose"/> is returned. For values between
/// 0 and 1 an interpolated pose is returned.
/// </param>
/// <returns>An interpolated pose.</returns>
public static Pose Interpolate(Pose startPose, Pose endPose, float parameter)
{
// Linearly interpolate position.
var interpolatedPosition = startPose.Position * (1 - parameter) + endPose.Position * parameter;
// Slerp orientation.
var interpolatedOrientation = InterpolationHelper.Lerp(
QuaternionF.CreateRotation(startPose.Orientation),
QuaternionF.CreateRotation(endPose.Orientation),
parameter);
return(new Pose(interpolatedPosition, interpolatedOrientation));
}
/// <summary>
/// Moves and rotates the scene node so that it faces a certain direction (in world space).
/// </summary>
/// <param name="node">The scene node.</param>
/// <param name="position">The new position in world space.</param>
/// <param name="target">
/// The target coordinates in world space at which the scene node is "looking".
/// </param>
/// <param name="upVector">
/// The direction that is "up" from the scene node's point of view given in world space. (Does
/// not need to be normalized.)
/// </param>
/// <remarks>
/// A scene node uses the same coordinate system as the <strong>XNA Framework</strong>:
/// By default, the positive x-axis points to the right, the positive y-axis points up, and the
/// positive z-axis points towards the viewer. This method moves the scene node to
/// <paramref name="position"/> and rotates it so that its local forward direction (0, 0, -1) is
/// pointing towards <paramref name="target"/>.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="node"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="position"/> is the same as <paramref name="target"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="upVector"/> is (0, 0, 0).
/// </exception>
/// <exception cref="DivideByZeroException">
/// The camera direction (<paramref name="target"/> - <paramref name="position"/>) is probably
/// pointing in the same or opposite direction as <paramref name="upVector"/>. (The two vectors
/// must not be parallel.)
/// </exception>
public static void LookAt(this SceneNode node, Vector3F position, Vector3F target, Vector3F upVector)
{
if (node == null)
{
throw new ArgumentNullException("node");
}
Matrix44F view = Matrix44F.CreateLookAt(position, target, upVector);
Matrix44F viewInverse = view.Inverse;
QuaternionF orientation = QuaternionF.CreateRotation(viewInverse.Minor);
node.PoseWorld = new Pose(position, orientation);
}
public void Update(float deltaTime, Matrix44F world)
{
if (deltaTime <= 0)
{
return;
}
// Reset bone transform.
SkeletonPose.SetBoneTransform(BoneIndex, SrtTransform.Identity);
// Get new fixed point position in world space.
var bonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(BoneIndex);
var bonePoseWorld = world * bonePoseAbsolute;
var fixedPointPosition = bonePoseWorld.TransformPosition(Offset);
// If we haven't set the fixed point position before, then store the position
// and we are done.
if (_fixedPointPosition.IsNaN)
{
_fixedPointPosition = fixedPointPosition;
return;
}
// New position and velocity of fixed point.
_fixedPointVelocity = (fixedPointPosition - _fixedPointPosition) / deltaTime;
_fixedPointPosition = fixedPointPosition;
// If the particle position was not set before, then we only store the current values.
// The real work starts in the next frame.
if (_particlePosition.IsNaN)
{
_particlePosition = _fixedPointPosition;
_particleVelocity = _fixedPointVelocity;
return;
}
// Compute the spring force between the particle and the fixed point.
var force = Spring * (_fixedPointPosition - _particlePosition) + Damping * (_fixedPointVelocity - _particleVelocity);
// Update velocity and position of the particle using symplectic Euler.
_particleVelocity = _particleVelocity + force * deltaTime;
_particlePosition = _particlePosition + _particleVelocity * deltaTime;
// Convert particle position back to bone space.
var particleLocal = bonePoseWorld.Inverse.TransformPosition(_particlePosition);
// Create rotation between the fixed point vector and the particle vector.
var boneTransform = new SrtTransform(QuaternionF.CreateRotation(Offset, particleLocal));
SkeletonPose.SetBoneTransform(BoneIndex, boneTransform);
}
public void GetAxisAlignedBoundingBox()
{
Assert.AreEqual(new Aabb(), new RectangleShape().GetAabb(Pose.Identity));
Assert.AreEqual(new Aabb(new Vector3F(10, 100, -13), new Vector3F(10, 100, -13)),
new RectangleShape().GetAabb(new Pose(new Vector3F(10, 100, -13),
QuaternionF.CreateRotation(new Vector3F(1, 1, 1), 0.7f))));
Assert.AreEqual(new Aabb(new Vector3F(5, 90, 1000), new Vector3F(15, 110, 1000)),
new RectangleShape(10, 20).GetAabb(new Pose(new Vector3F(10, 100, 1000),
QuaternionF.Identity)));
Assert.AreEqual(new Aabb(new Vector3F(5, 100, 990), new Vector3F(15, 100, 1010)),
new RectangleShape(10, 20).GetAabb(new Pose(new Vector3F(10, 100, 1000),
QuaternionF.CreateRotationX(ConstantsF.PiOver2))));
// TODO: Test complex rotations.
}
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);
}
public void GetAxisAlignedBoundingBox()
{
float nInf = float.NegativeInfinity;
float pInf = float.PositiveInfinity;
Assert.AreEqual(new Aabb(new Vector3F(nInf, 0, 0), new Vector3F(pInf, 0, 0)), new LineShape().GetAabb(Pose.Identity));
Assert.AreEqual(new Aabb(new Vector3F(nInf), new Vector3F(pInf)),
new LineShape().GetAabb(new Pose(new Vector3F(10, 100, -13),
QuaternionF.CreateRotation(new Vector3F(1, 1, 1), 0.7f))));
Assert.AreEqual(new Aabb(new Vector3F(11, nInf, 1003), new Vector3F(11, pInf, 1003)),
new LineShape(new Vector3F(1, 2, 3), new Vector3F(0, -1, 0)).GetAabb(new Pose(new Vector3F(10, 100, 1000),
QuaternionF.Identity)));
// TODO: Test rotations.
}
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
private void InitializeBody(Vector3F upVector)
{
if (!upVector.TryNormalize())
{
throw new ArgumentException("The up vector must not be a zero vector.");
}
UpVector = upVector;
CapsuleShape shape = new CapsuleShape(0.4f, 1.8f);
MassFrame mass = new MassFrame {
Mass = 100
};
UniformMaterial material = new UniformMaterial
{
// The body should be frictionless, so that it can be easily pushed by the simulation to
// valid positions.
StaticFriction = 0.0f,
DynamicFriction = 0.0f,
// The body should not bounce when being hit or pushed.
Restitution = 0
};
Body = new RigidBody(shape, mass, material)
{
// We set the mass explicitly and it should not automatically change when the
// shape is changed; e.g. a ducked character has a smaller shape, but still the same mass.
AutoUpdateMass = false,
// This body is under our control and should never be deactivated by the simulation.
CanSleep = false,
CcdEnabled = true,
// The capsule does not rotate in any direction.
LockRotationX = true,
LockRotationY = true,
LockRotationZ = true,
Name = "CharacterController",
Pose = new Pose(shape.Height / 2 * upVector,
QuaternionF.CreateRotation(Vector3F.UnitY, upVector)),
};
// When the user changes the shape, we must re-compute all contacts.
Body.ShapeChanged += (s, e) => UpdateContacts();
}
public void GetAxisAlignedBoundingBox()
{
Assert.AreEqual(new Aabb(), new PointShape().GetAabb(Pose.Identity));
Assert.AreEqual(new Aabb(new Vector3F(10, 100, -13), new Vector3F(10, 100, -13)),
new PointShape().GetAabb(new Pose(new Vector3F(10, 100, -13),
QuaternionF.CreateRotation(new Vector3F(1, 1, 1), 0.7f))));
Assert.AreEqual(new Aabb(new Vector3F(11, 102, 1003), new Vector3F(11, 102, 1003)),
new PointShape(new Vector3F(1, 2, 3)).GetAabb(new Pose(new Vector3F(10, 100, 1000),
QuaternionF.Identity)));
QuaternionF rotation = QuaternionF.CreateRotation(new Vector3F(1, 1, 1), 0.7f);
Vector3F worldPos = rotation.Rotate(new Vector3F(1, 2, 3)) + new Vector3F(10, 100, 1000);
Assert.IsTrue(Vector3F.AreNumericallyEqual(worldPos, new PointShape(new Vector3F(1, 2, 3)).GetAabb(new Pose(new Vector3F(10, 100, 1000), rotation)).Minimum));
Assert.IsTrue(Vector3F.AreNumericallyEqual(worldPos, new PointShape(new Vector3F(1, 2, 3)).GetAabb(new Pose(new Vector3F(10, 100, 1000), rotation)).Maximum));
}
public void QuaternionFromMatrix33()
{
Vector3F v = Vector3F.One;
Matrix33F m = Matrix33F.Identity;
QuaternionF q = QuaternionF.CreateRotation(m);
Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));
m = Matrix33F.CreateRotation(Vector3F.UnitX, 0.3f);
q = QuaternionF.CreateRotation(m);
Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));
m = Matrix33F.CreateRotation(Vector3F.UnitY, 1.0f);
q = QuaternionF.CreateRotation(m);
Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));
m = Matrix33F.CreateRotation(Vector3F.UnitZ, 4.0f);
q = QuaternionF.CreateRotation(m);
Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));
m = Matrix33F.Identity;
q = QuaternionF.CreateRotation(m);
Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));
m = Matrix33F.CreateRotation(-Vector3F.UnitX, 1.3f);
q = QuaternionF.CreateRotation(m);
Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));
m = Matrix33F.CreateRotation(-Vector3F.UnitY, -1.4f);
q = QuaternionF.CreateRotation(m);
Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));
m = Matrix33F.CreateRotation(-Vector3F.UnitZ, -0.1f);
q = QuaternionF.CreateRotation(m);
Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));
m = new Matrix33F(0, 0, 1,
0, -1, 0,
1, 0, 0);
q = QuaternionF.CreateRotation(m);
Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));
m = new Matrix33F(-1, 0, 0,
0, 1, 0,
0, 0, -1);
q = QuaternionF.CreateRotation(m);
Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));
}
public void GetAxisAlignedBoundingBox()
{
Assert.AreEqual(new Aabb(), new Aabb().GetAabb(Pose.Identity));
Assert.AreEqual(new Aabb(new Vector3F(10, 100, 1000), new Vector3F(10, 100, 1000)),
new Aabb().GetAabb(new Pose(new Vector3F(10, 100, 1000), QuaternionF.Identity)));
Assert.AreEqual(new Aabb(new Vector3F(10, 100, 1000), new Vector3F(10, 100, 1000)),
new Aabb().GetAabb(new Pose(new Vector3F(10, 100, 1000), QuaternionF.CreateRotation(new Vector3F(1, 2, 3), 0.7f))));
Aabb aabb = new Aabb(new Vector3F(1, 10, 100), new Vector3F(2, 20, 200));
Assert.AreEqual(aabb, aabb.GetAabb(Pose.Identity));
Assert.AreEqual(new Aabb(new Vector3F(11, 110, 1100), new Vector3F(12, 120, 1200)),
aabb.GetAabb(new Pose(new Vector3F(10, 100, 1000), QuaternionF.Identity)));
// TODO: Test rotations.
}
// In this method, a vector is rotated with a quaternion and a matrix. The result
// of the two vector rotations are compared.
private void RotateVector()
{
var debugRenderer = GraphicsScreen.DebugRenderer2D;
debugRenderer.DrawText("----- RotateVector Example:");
// Create a vector. We will rotate this vector.
Vector3F v = new Vector3F(1, 2, 3);
// Create another vector which defines the axis of a rotation.
Vector3F rotationAxis = Vector3F.UnitZ;
// The rotation angle in radians. We want to rotate 50°.
float rotationAngle = MathHelper.ToRadians(50);
// ----- Part 1: Rotate a vector with a quaternion.
// Create a quaternion that represents a 50° rotation around the axis given
// by rotationAxis.
QuaternionF rotation = QuaternionF.CreateRotation(rotationAxis, rotationAngle);
// Rotate the vector v using the rotation quaternion.
Vector3F vRotated = rotation.Rotate(v);
// ----- Part 2: Rotate a vector with a matrix.
// Create a matrix that represents a 50° rotation around the axis given by
// rotationAxis.
Matrix33F rotationMatrix = Matrix33F.CreateRotation(rotationAxis, rotationAngle);
// Rotate the vector v using the rotation matrix.
Vector3F vRotated2 = rotationMatrix * v;
// ----- Part 3: Compare the results.
// The result of both rotations should be identical.
// Because of numerical errors there can be minor differences in the results.
// Therefore we use Vector3F.AreNumericallyEqual() two check if the results
// are equal (within a sensible numerical tolerance).
if (Vector3F.AreNumericallyEqual(vRotated, vRotated2))
{
debugRenderer.DrawText("Vectors are equal.\n"); // This message is written.
}
else
{
debugRenderer.DrawText("Vectors are not equal.\n");
}
}
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)));
}
/// <summary>
/// Called when <see cref="BoneMapper.MapAToB"/> was called.
/// </summary>
protected override void OnMapAToB()
{
CacheDerivedData();
var skeletonInstanceA = SkeletonMapper.SkeletonPoseA;
var skeletonInstanceB = SkeletonMapper.SkeletonPoseB;
// ----- Set start transform.
// Note: It is important to start from an absolute bone pose that does not depend
// on the last frame. Otherwise, errors accumulate and the bone chain starts to twist.
if (MapFromBindPose)
{
// Reset the bone rotation of the root bone to start from the bind pose.
skeletonInstanceB.ResetBoneTransforms(RootBoneIndexB, RootBoneIndexB, false, true, false);
}
else
{
// Start from a direct-mapped bone pose.
_directBoneMapper.MapAToB();
}
// Get absolute bone poses.
var rootBoneA = skeletonInstanceA.GetBonePoseAbsolute(RootBoneIndexA);
var tipBoneA = skeletonInstanceA.GetBonePoseAbsolute(TipBoneIndexA);
var rootBoneB = skeletonInstanceB.GetBonePoseAbsolute(RootBoneIndexB);
var tipBoneB = skeletonInstanceB.GetBonePoseAbsolute(TipBoneIndexB);
// Compute direction vector for the two chains (origin to tip).
var directionB = tipBoneB.Translation - rootBoneB.Translation;
var directionA = tipBoneA.Translation - rootBoneA.Translation;
// Abort if any chain has zero length.
if (directionB.IsNumericallyZero || directionA.IsNumericallyZero)
{
return;
}
// Apply global skeleton rotation offset to rotate all into model B space.
directionA = SkeletonMapper.RotationOffset.Rotate(directionA);
// Compute and apply rotation between the two direction vectors.
var rotation = QuaternionF.CreateRotation(directionB, directionA);
skeletonInstanceB.RotateBoneAbsolute(RootBoneIndexB, rotation);
}
private void LimitBoneTransform()
{
// This method is called by the JacobianTransposeIKSolver after each internal iteration.
// The job of this method is to apply bone limits; for example, the elbow should not
// bend backwards, etc.
// To apply a limit, get the bone transform or bone pose from skeleton pose, check if
// is in the allowed range. If it is outside the allowed range, rotate it back to the
// nearest allowed rotation.
// Here, for example, we only make sure that the palm of the hand is always parallel
// to the ground plane - as if the character wants to grab a horizontal bar or as
// if it wants to place the hand on horizontal plane.
SrtTransform bonePoseAbsolute = _meshNode.SkeletonPose.GetBonePoseAbsolute(15);
Vector3F palmAxis = bonePoseAbsolute.ToParentDirection(-Vector3F.UnitY);
bonePoseAbsolute.Rotation = QuaternionF.CreateRotation(palmAxis, Vector3F.UnitY) * bonePoseAbsolute.Rotation;
_meshNode.SkeletonPose.SetBonePoseAbsolute(15, bonePoseAbsolute);
}
public void Division()
{
float angle1 = 0.4f;
Vector3F axis1 = new Vector3F(1.0f, 2.0f, 3.0f);
QuaternionF q1 = QuaternionF.CreateRotation(axis1, angle1);
Matrix33F m1 = Matrix33F.CreateRotation(axis1, angle1);
float angle2 = -1.6f;
Vector3F axis2 = new Vector3F(1.0f, -2.0f, -3.5f);
QuaternionF q2 = QuaternionF.CreateRotation(axis2, angle2);
Matrix33F m2 = Matrix33F.CreateRotation(axis2, angle2);
Vector3F v = new Vector3F(0.3f, -2.4f, 5.6f);
Vector3F result1 = QuaternionF.Divide(q2, q1).Rotate(v);
Vector3F result2 = m2 * m1.Inverse * v;
Assert.IsTrue(Vector3F.AreNumericallyEqual(result1, result2));
}
public void LerpQuaternionF()
{
// Warning: The not all results are not verified
QuaternionF q1 = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f).Normalized;
QuaternionF q2 = new QuaternionF(2.0f, 4.0f, 6.0f, 8.0f).Normalized;
QuaternionF lerp = InterpolationHelper.Lerp(q1, q2, 0.75f);
Assert.IsTrue(lerp.IsNumericallyNormalized);
lerp = InterpolationHelper.Lerp(q1, q2, 0);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(q1, lerp));
lerp = InterpolationHelper.Lerp(q1, q2, 1);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(q2, lerp));
q1 = QuaternionF.Identity;
q2 = QuaternionF.CreateRotation(Vector3F.UnitZ, (float)Math.PI / 2);
lerp = InterpolationHelper.Lerp(q1, q2, 0.5f);
Vector3F v = lerp.Rotate(Vector3F.UnitX);
Vector3F result = new Vector3F(1.0f, 1.0f, 0.0f).Normalized;
Assert.IsTrue(Vector3F.AreNumericallyEqual(result, v));
q1 = QuaternionF.Identity;
q2 = QuaternionF.CreateRotation(Vector3F.UnitY, (float)Math.PI / 2);
lerp = InterpolationHelper.Lerp(q1, q2, 0.5f);
v = lerp.Rotate(Vector3F.UnitZ);
result = new Vector3F(1.0f, 0.0f, 1.0f).Normalized;
Assert.IsTrue(Vector3F.AreNumericallyEqual(result, v));
q1 = QuaternionF.Identity;
q2 = QuaternionF.CreateRotation(Vector3F.UnitX, (float)Math.PI / 2);
lerp = InterpolationHelper.Lerp(q1, q2, 0.5f);
v = lerp.Rotate(Vector3F.UnitY);
result = new Vector3F(0.0f, 1.0f, 1.0f).Normalized;
Assert.IsTrue(Vector3F.AreNumericallyEqual(result, v));
q1 = new QuaternionF(-1.0f, 0.0f, 0.0f, 0.0f);
q2 = QuaternionF.CreateRotation(-Vector3F.UnitZ, (float)-Math.PI / 2);
lerp = InterpolationHelper.Lerp(q1, q2, 0.5f);
v = lerp.Rotate(Vector3F.UnitX);
result = new Vector3F(1.0f, 1.0f, 0.0f).Normalized;
Assert.IsTrue(Vector3F.AreNumericallyEqual(result, v));
}
public void ComputeAngularVelocity()
{
var v = new Vector3F(0.1f, 0.2f, 0.3f);
var o0 = QuaternionF.CreateRotation(new Vector3F(-7, 8, 0.3f).Normalized, 4.2f);
var dt = 1 / 60f;
var o1 = (QuaternionF.CreateRotation(v, v.Length * dt) * o0).Normalized;
// Negate quaternion. This is still the same rotation, but now we can test if
// the rotation around the shortest arc is used.
o1 = -o1;
// We use a big epsilon. Quaternion multiplication seems to create a large error...?
Assert.IsTrue(Vector3F.AreNumericallyEqual(v, AnimationHelper.ComputeAngularVelocity(o0, o1, dt), 0.01f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(v, AnimationHelper.ComputeAngularVelocity(o0.ToRotationMatrix33(), o1.ToRotationMatrix33(), dt), 0.01f));
// Zero rotation.
Assert.AreEqual(Vector3F.Zero, AnimationHelper.ComputeAngularVelocity(o0, -o0, dt));
}
public void Axis()
{
Vector3F axis = new Vector3F(1.0f, 2.0f, 3.0f);
float angle = 0.2f;
QuaternionF q = QuaternionF.CreateRotation(axis, angle);
Assert.IsTrue(Numeric.AreEqual(angle, q.Angle));
Assert.IsTrue(Vector3F.AreNumericallyEqual(axis.Normalized, q.Axis));
axis = new Vector3F(1.0f, 1.0f, 1.0f);
q.Axis = axis;
Assert.IsTrue(Numeric.AreEqual(angle, q.Angle));
Assert.IsTrue(Vector3F.AreNumericallyEqual(axis.Normalized, q.Axis));
Assert.IsTrue(Vector3F.AreNumericallyEqual(Matrix33F.CreateRotation(axis, angle) * Vector3F.One, q.Rotate(Vector3F.One)));
Assert.AreEqual(Vector3F.Zero, QuaternionF.Identity.Axis);
q.Axis = Vector3F.Zero;
Assert.AreEqual(QuaternionF.Identity, q);
}
public void Inverse()
{
QuaternionF identity = QuaternionF.Identity;
QuaternionF inverseIdentity = identity.Inverse;
Assert.AreEqual(inverseIdentity, identity);
float angle = 0.4f;
Vector3F axis = new Vector3F(1.0f, 1.0f, 1.0f);
axis.Normalize();
QuaternionF q = QuaternionF.CreateRotation(axis, angle);
QuaternionF inverse = q.Inverse;
Assert.IsTrue(Vector3F.AreNumericallyEqual(-axis, inverse.Axis));
q = new QuaternionF(1, 2, 3, 4);
inverse = q.Inverse;
Assert.IsTrue(QuaternionF.AreNumericallyEqual(QuaternionF.Identity, inverse * q));
}
public void SlerpGeneralSinglePrecision()
{
QuaternionF q1 = QuaternionF.CreateRotation(-Vector3F.UnitY, (float)Math.PI / 2);
QuaternionF q2 = QuaternionF.CreateRotation(Vector3F.UnitZ, (float)Math.PI / 2);
QuaternionF slerp = InterpolationHelper.Slerp(q1, q2, 0.5f);
Assert.IsTrue(slerp.IsNumericallyNormalized);
Vector3F v = slerp.Rotate(Vector3F.UnitX);
Vector3F result = new Vector3F(1.0f / 3.0f, 2.0f / 3.0f, 2.0f / 3.0f); // I hope this is correct.
Assert.IsTrue(Vector3F.AreNumericallyEqual(result, v));
q1 = QuaternionF.CreateRotation(-Vector3F.UnitY, (float)Math.PI / 2);
q2 = QuaternionF.CreateRotation(-Vector3F.UnitZ, (float)-Math.PI / 2);
slerp = InterpolationHelper.Slerp(q1, q2, 0.5f);
Assert.IsTrue(slerp.IsNumericallyNormalized);
v = slerp.Rotate(Vector3F.UnitX);
result = new Vector3F(1.0f / 3.0f, 2.0f / 3.0f, 2.0f / 3.0f); // I hope this is correct.
Assert.IsTrue(Vector3F.AreNumericallyEqual(result, v));
}
private static void InsertRandomKeyFrames(Random random, SrtKeyFrameAnimation animation, TimeSpan time0, TimeSpan time1,
float scaleThreshold, float rotationThreshold, float translationThreshold)
{
rotationThreshold = MathHelper.ToRadians(rotationThreshold);
var defaultSource = SrtTransform.Identity;
var defaultTarget = SrtTransform.Identity;
var value = new SrtTransform();
int insertionIndex = 0;
for (int i = 0; i < animation.KeyFrames.Count; i++)
{
if (animation.KeyFrames[i].Time == time0)
{
insertionIndex = i + 1;
break;
}
}
Debug.Assert(insertionIndex > 0);
const int numberOfKeyFrames = 2;
long tickIncrement = (time1 - time0).Ticks / (numberOfKeyFrames + 1);
for (int i = 0; i < numberOfKeyFrames; i++)
{
var time = TimeSpan.FromTicks(time0.Ticks + (i + 1) * tickIncrement);
Debug.Assert(time0 < time && time < time1);
// Get interpolated animation value.
animation.GetValue(time, ref defaultSource, ref defaultTarget, ref value);
// Apply small variation (within thresholds).
value.Scale += random.NextVector3F(-1, 1).Normalized *(scaleThreshold / 2);
value.Rotation = QuaternionF.CreateRotation(random.NextVector3F(-1, 1), rotationThreshold / 2) * value.Rotation;
value.Translation += random.NextVector3F(-1, 1).Normalized *(translationThreshold / 2);
animation.KeyFrames.Insert(insertionIndex, new KeyFrame <SrtTransform>(time, value));
insertionIndex++;
}
}