/// <inheritdoc/>
public override Aabb GetAabb(Vector3F scale, Pose pose)
{
if (scale.X == scale.Y && scale.Y == scale.Z)
{
// Uniform scaling.
// Get world axes in local space. They are equal to the rows of the orientation matrix.
Matrix33F rotationMatrix = pose.Orientation;
Vector3F worldX = rotationMatrix.GetRow(0);
Vector3F worldY = rotationMatrix.GetRow(1);
Vector3F worldZ = rotationMatrix.GetRow(2);
// Get extreme points along positive axes.
Vector3F halfExtent = Vector3F.Absolute(
new Vector3F(
Vector3F.Dot(GetSupportPointNormalized(worldX), worldX),
Vector3F.Dot(GetSupportPointNormalized(worldY), worldY),
Vector3F.Dot(GetSupportPointNormalized(worldZ), worldZ)));
// Apply scale.
halfExtent *= Math.Abs(scale.X);
Vector3F minimum = pose.Position - halfExtent;
Vector3F maximum = pose.Position + halfExtent;
Debug.Assert(minimum <= maximum);
return(new Aabb(minimum, maximum));
}
else
{
// Non-uniform scaling.
return(base.GetAabb(scale, pose));
}
}
/// <summary>
/// Gets the Euler angles for the given rotation.
/// </summary>
/// <param name="rotation">The rotation.</param>
/// <returns>
/// A vector with the three Euler angles in radians: (angle0, angle1, angle2).
/// </returns>
/// <remarks>
/// <para>
/// The Euler angles are computed for following order of rotations: The first rotations is about
/// the x-axis. The second rotation is about the rotated y-axis after the first rotation. The
/// last rotation is about the final z-axis.
/// </para>
/// <para>
/// The Euler angles are unique if the second angle is less than +/- 90°. The limits for the
/// rotation angles are [-180°, 180°] for the first and the third angle. And the limit for the
/// second angle is [-90°, 90°].
/// </para>
/// </remarks>
public static Vector3F GetEulerAngles(Matrix33F rotation)
{
// See book Geometric Tools, "Factoring Rotation Matrices as RxRyRz", pp. 848.
Vector3F result = new Vector3F();
float sinY = rotation.M02;
if (sinY < 1.0f)
{
if (sinY > -1.0f)
{
result.X = (float)Math.Atan2(-rotation.M12, rotation.M22);
result.Y = (float)Math.Asin(sinY);
result.Z = (float)Math.Atan2(-rotation.M01, rotation.M00);
}
else
{
// Not a unique solution (thetaX - thetaZ is constant).
result.X = -(float)Math.Atan2(rotation.M10, rotation.M11);
result.Y = -ConstantsF.PiOver2;
result.Z = 0;
}
}
else
{
// Not a unique solution (thetaX + thetaZ is constant).
result.X = (float)Math.Atan2(rotation.M10, rotation.M11);
result.Y = ConstantsF.PiOver2;
result.Z = 0;
}
return(result);
}
//--------------------------------------------------------------
#region Creation & Cleanup
//--------------------------------------------------------------
#endregion
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
/// <inheritdoc/>
protected override void OnSetup()
{
var deltaTime = Simulation.Settings.Timing.FixedTimeStep;
float errorReduction = ConstraintHelper.ComputeErrorReduction(deltaTime, SpringConstant, DampingConstant);
float softness = ConstraintHelper.ComputeSoftness(deltaTime, SpringConstant, DampingConstant);
// Get anchor orientations in world space.
Matrix33F anchorOrientationA = BodyA.Pose.Orientation * AnchorOrientationALocal;
Matrix33F anchorOrientationB = BodyB.Pose.Orientation * AnchorOrientationBLocal;
Matrix33F relativeOrientation = anchorOrientationA.Transposed * anchorOrientationB;
Vector3F angles = ConstraintHelper.GetEulerAngles(relativeOrientation);
// The constraint axes: See OneNote for a detailed derivation of these non-intuitive axes.
var xA = anchorOrientationA.GetColumn(0); // Anchor x-axis on A.
var zB = anchorOrientationB.GetColumn(2); // Anchor z-axis on B.
Vector3F constraintAxisY = Vector3F.Cross(zB, xA);
Vector3F constraintAxisX = Vector3F.Cross(constraintAxisY, zB);
Vector3F constraintAxisZ = Vector3F.Cross(xA, constraintAxisY);
SetupConstraint(0, angles[0], TargetAngles[0], constraintAxisX, deltaTime, errorReduction, softness);
SetupConstraint(1, angles[1], TargetAngles[1], constraintAxisY, deltaTime, errorReduction, softness);
SetupConstraint(2, angles[2], TargetAngles[2], constraintAxisZ, deltaTime, errorReduction, softness);
// No warmstarting.
_constraints[0].ConstraintImpulse = 0;
_constraints[1].ConstraintImpulse = 0;
_constraints[2].ConstraintImpulse = 0;
}
/// <inheritdoc/>
protected override void OnSetup()
{
// Get anchor pose/position in world space.
Pose anchorPoseA = BodyA.Pose * AnchorPoseALocal;
Vector3F anchorPositionB = BodyB.Pose.ToWorldPosition(AnchorPositionBLocal); // TODO: We could store rALocal and use ToWorldDirection instead of ToWorldPosition.
// Compute anchor position on B relative to anchor pose of A.
Vector3F relativePosition = anchorPoseA.ToLocalPosition(anchorPositionB);
// The linear constraint axes are the fixed anchor axes of A!
Matrix33F anchorOrientation = anchorPoseA.Orientation;
Vector3F rA = anchorPoseA.Position - BodyA.PoseCenterOfMass.Position;
Vector3F rB = anchorPositionB - BodyB.PoseCenterOfMass.Position;
// Remember old states.
LimitState oldXLimitState = _limitStates[0];
LimitState oldYLimitState = _limitStates[1];
LimitState oldZLimitState = _limitStates[2];
SetupConstraint(0, relativePosition.X, anchorOrientation.GetColumn(0), rA, rB);
SetupConstraint(1, relativePosition.Y, anchorOrientation.GetColumn(1), rA, rB);
SetupConstraint(2, relativePosition.Z, anchorOrientation.GetColumn(2), rA, rB);
Warmstart(0, oldXLimitState);
Warmstart(1, oldYLimitState);
Warmstart(2, oldZLimitState);
}
/// <inheritdoc/>
public override Aabb GetAabb(Vector3F scale, Pose pose)
{
Vector3F halfExtent = new Vector3F(_widthX / 2, _widthY / 2, _widthZ / 2) * Vector3F.Absolute(scale);
if (pose == Pose.Identity)
{
return(new Aabb(-halfExtent, halfExtent));
}
// Get world axes in local space. They are equal to the rows of the orientation matrix.
Matrix33F rotationMatrix = pose.Orientation;
Vector3F worldX = rotationMatrix.GetRow(0);
Vector3F worldY = rotationMatrix.GetRow(1);
Vector3F worldZ = rotationMatrix.GetRow(2);
// The half extent vector is in the +x/+y/+z octant of the world. We want to project
// the extent onto the world axes. The half extent projected onto world x gives us the
// x extent.
// The world axes in local space could be in another world octant. We could now either find
// out the in which octant the world axes is pointing and build the correct half extent vector
// for this octant. OR we mirror the world axis vectors into the +x/+y/+z octant by taking
// the absolute vector.
worldX = Vector3F.Absolute(worldX);
worldY = Vector3F.Absolute(worldY);
worldZ = Vector3F.Absolute(worldZ);
// Now we project the extent onto the world axes.
Vector3F halfExtentWorld = new Vector3F(Vector3F.Dot(halfExtent, worldX),
Vector3F.Dot(halfExtent, worldY),
Vector3F.Dot(halfExtent, worldZ));
return(new Aabb(pose.Position - halfExtentWorld, pose.Position + halfExtentWorld));
}
/// <inheritdoc/>
protected override void OnSetup()
{
// Get anchor orientations in world space.
Matrix33F anchorOrientationA = BodyA.Pose.Orientation * AnchorOrientationALocal;
Matrix33F anchorOrientationB = BodyB.Pose.Orientation * AnchorOrientationBLocal;
// Get anchor orientation of B relative to A.
Matrix33F relativeOrientation = anchorOrientationA.Transposed * anchorOrientationB;
Vector3F angles = ConstraintHelper.GetEulerAngles(relativeOrientation);
// The constraint axes: See OneNote for a detailed derivation of these non-intuitive axes.
var xA = anchorOrientationA.GetColumn(0); // Anchor x-axis on A.
var zB = anchorOrientationB.GetColumn(2); // Anchor z-axis on B.
Vector3F constraintAxisY = Vector3F.Cross(zB, xA);
Vector3F constraintAxisX = Vector3F.Cross(constraintAxisY, zB);
Vector3F constraintAxisZ = Vector3F.Cross(xA, constraintAxisY);
// Remember old states.
LimitState oldXLimitState = _limitStates[0];
LimitState oldYLimitState = _limitStates[1];
LimitState oldZLimitState = _limitStates[2];
SetupConstraint(0, angles.X, constraintAxisX);
SetupConstraint(1, angles.Y, constraintAxisY);
SetupConstraint(2, angles.Z, constraintAxisZ);
Warmstart(0, oldXLimitState);
Warmstart(1, oldYLimitState);
Warmstart(2, oldZLimitState);
}
public void IsRotation()
{
Assert.IsTrue(!Matrix33F.Zero.IsRotation);
Assert.IsTrue(Matrix33F.Identity.IsRotation);
Assert.IsTrue(Matrix33F.CreateRotation(new Vector3F(1, 2, 3).Normalized, 0.5f).IsRotation);
Assert.IsTrue(!new Matrix33F(1, 0, 0, 0, 1, 0, 0, 0, -1).IsRotation);
}
protected override Pose Read(ContentReader input, Pose existingInstance)
{
Vector3F position = input.ReadRawObject <Vector3F>();
Matrix33F orientation = input.ReadRawObject <Matrix33F>();
return(new Pose(position, orientation));
}
/// <summary>
/// Changes the mass to the given target mass.
/// </summary>
/// <param name="targetMass">The target mass.</param>
/// <param name="mass">The mass.</param>
/// <param name="inertia">The inertia.</param>
private static void AdjustMass(float targetMass, ref float mass, ref Matrix33F inertia)
{
float scale = targetMass / mass;
mass = targetMass;
inertia = inertia * scale;
}
// Add a chain-like figure using TransformedFigure and CompositeFigure.
private void CreateChain()
{
var ellipse = new EllipseFigure
{
IsFilled = false,
RadiusX = 1f,
RadiusY = 1f,
};
var compositeFigure = new CompositeFigure();
for (int i = 0; i < 9; i++)
{
var transformedEllipse = new TransformedFigure(ellipse)
{
Scale = new Vector3F(0.4f, 0.2f, 1),
Pose = new Pose(new Vector3F(-2 + i * 0.5f, 0, 0), Matrix33F.CreateRotationX(ConstantsF.PiOver2 * (i % 2)))
};
compositeFigure.Children.Add(transformedEllipse);
}
_scene.Children.Add(new FigureNode(compositeFigure)
{
Name = "Chain",
StrokeThickness = 2,
StrokeColor = new Vector3F(0.1f),
StrokeAlpha = 1,
PoseLocal = new Pose(new Vector3F(0, 3, 0)),
});
}
public void SerializationBinary()
{
Matrix33F m1 = new Matrix33F(12, 23, 45,
56, 67, 89,
90, 12, 43.3f);
Matrix33F m2;
string fileName = "SerializationMatrix33F.bin";
if (File.Exists(fileName))
{
File.Delete(fileName);
}
FileStream fs = new FileStream(fileName, FileMode.Create);
BinaryFormatter formatter = new BinaryFormatter();
formatter.Serialize(fs, m1);
fs.Close();
fs = new FileStream(fileName, FileMode.Open);
formatter = new BinaryFormatter();
m2 = (Matrix33F)formatter.Deserialize(fs);
fs.Close();
Assert.AreEqual(m1, m2);
}
public void ToLocal()
{
Vector3F point0 = new Vector3F(1, 0.5f, 0.5f);
Vector3F point1 = new Vector3F(0.5f, 1, 0.5f);
Vector3F point2 = new Vector3F(0.5f, 0.5f, 1);
Plane plane = new Plane(point0, point1, point2);
Vector3F pointAbove = plane.Normal * plane.DistanceFromOrigin * 2;
Vector3F pointBelow = plane.Normal * plane.DistanceFromOrigin * 0.5f;
Assert.IsTrue(Vector3F.Dot(plane.Normal, pointAbove) > plane.DistanceFromOrigin);
Assert.IsTrue(Vector3F.Dot(plane.Normal, pointBelow) < plane.DistanceFromOrigin);
Pose pose = new Pose(new Vector3F(-5, 100, -20), Matrix33F.CreateRotation(new Vector3F(1, 2, 3), 0.123f));
point0 = pose.ToLocalPosition(point0);
point1 = pose.ToLocalPosition(point1);
point2 = pose.ToLocalPosition(point2);
pointAbove = pose.ToLocalPosition(pointAbove);
pointBelow = pose.ToLocalPosition(pointBelow);
plane.ToLocal(ref pose);
Assert.IsTrue(plane.Normal.IsNumericallyNormalized);
Vector3F dummy;
Assert.IsTrue(GeometryHelper.GetClosestPoint(plane, point0, out dummy));
Assert.IsTrue(GeometryHelper.GetClosestPoint(plane, point1, out dummy));
Assert.IsTrue(GeometryHelper.GetClosestPoint(plane, point2, out dummy));
Assert.IsTrue(Vector3F.Dot(plane.Normal, pointAbove) > plane.DistanceFromOrigin);
Assert.IsTrue(Vector3F.Dot(plane.Normal, pointBelow) < plane.DistanceFromOrigin);
}
public void MultiplyTransposed()
{
var m = RandomHelper.Random.NextMatrix33F(1, 10);
var v = RandomHelper.Random.NextVector3F(1, 10);
Assert.AreEqual(m.Transposed * v, Matrix33F.MultiplyTransposed(m, v));
}
public void SerializationXml()
{
Matrix33F m1 = new Matrix33F(12, 23, 45,
67, 89, 90,
43, 65, 87.3f);
Matrix33F m2;
string fileName = "SerializationMatrix33F.xml";
if (File.Exists(fileName))
{
File.Delete(fileName);
}
XmlSerializer serializer = new XmlSerializer(typeof(Matrix33F));
StreamWriter writer = new StreamWriter(fileName);
serializer.Serialize(writer, m1);
writer.Close();
serializer = new XmlSerializer(typeof(Matrix33F));
FileStream fileStream = new FileStream(fileName, FileMode.Open);
m2 = (Matrix33F)serializer.Deserialize(fileStream);
Assert.AreEqual(m1, m2);
}
public void MultiplyMatrix()
{
Matrix33F m = new Matrix33F(12, 23, 45,
67, 89, 90,
43, 65, 87);
Assert.AreEqual(Matrix33F.Zero, Matrix33F.Multiply(m, Matrix33F.Zero));
Assert.AreEqual(Matrix33F.Zero, Matrix33F.Multiply(Matrix33F.Zero, m));
Assert.AreEqual(m, Matrix33F.Multiply(m, Matrix33F.Identity));
Assert.AreEqual(m, Matrix33F.Multiply(Matrix33F.Identity, m));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(Matrix33F.Identity, Matrix33F.Multiply(m, m.Inverse)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(Matrix33F.Identity, Matrix33F.Multiply(m.Inverse, m)));
Matrix33F m1 = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
Matrix33F m2 = new Matrix33F(12, 23, 45,
67, 89, 90,
43, 65, 87);
Matrix33F result = Matrix33F.Multiply(m1, m2);
for (int column = 0; column < 3; column++)
{
for (int row = 0; row < 3; row++)
{
Assert.AreEqual(Vector3F.Dot(m1.GetRow(row), m2.GetColumn(column)), result[row, column]);
}
}
}
public void AbsoluteStatic()
{
Matrix33F m = new Matrix33F(-1, -2, -3, -4, -5, -6, -7, -8, -9);
Matrix33F absoluteM = Matrix33F.Absolute(m);
Assert.AreEqual(1, absoluteM.M00);
Assert.AreEqual(2, absoluteM.M01);
Assert.AreEqual(3, absoluteM.M02);
Assert.AreEqual(4, absoluteM.M10);
Assert.AreEqual(5, absoluteM.M11);
Assert.AreEqual(6, absoluteM.M12);
Assert.AreEqual(7, absoluteM.M20);
Assert.AreEqual(8, absoluteM.M21);
Assert.AreEqual(9, absoluteM.M22);
m = new Matrix33F(1, 2, 3, 4, 5, 6, 7, 8, 9);
absoluteM = Matrix33F.Absolute(m);
Assert.AreEqual(1, absoluteM.M00);
Assert.AreEqual(2, absoluteM.M01);
Assert.AreEqual(3, absoluteM.M02);
Assert.AreEqual(4, absoluteM.M10);
Assert.AreEqual(5, absoluteM.M11);
Assert.AreEqual(6, absoluteM.M12);
Assert.AreEqual(7, absoluteM.M20);
Assert.AreEqual(8, absoluteM.M21);
Assert.AreEqual(9, absoluteM.M22);
}
public SmokeSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Create a single particle system and add it multiple times to the scene
// graph ("instancing"). By default, all instances look identical. The
// properties ParticleSystemNode.Color/Alpha/AngleOffset can be used to
// render the particles with some variations.
var particleSystem = Smoke.Create(ContentManager);
ParticleSystemService.ParticleSystems.Add(particleSystem);
_particleSystemNode0 = new ParticleSystemNode(particleSystem);
GraphicsScreen.Scene.Children.Add(_particleSystemNode0);
_particleSystemNode1 = new ParticleSystemNode(particleSystem);
_particleSystemNode1.PoseWorld = new Pose(new Vector3F(5, 0, -5));
_particleSystemNode1.Color = new Vector3F(0.9f, 0.8f, 0.7f);
_particleSystemNode1.Alpha = 0.8f;
_particleSystemNode1.AngleOffset = 0.3f;
GraphicsScreen.Scene.Children.Add(_particleSystemNode1);
_particleSystemNode2 = new ParticleSystemNode(particleSystem);
_particleSystemNode2.PoseWorld = new Pose(new Vector3F(-10, 5, -5), Matrix33F.CreateRotationZ(-ConstantsF.PiOver2));
_particleSystemNode2.Color = new Vector3F(0.5f, 0.5f, 0.5f);
_particleSystemNode2.AngleOffset = 0.6f;
GraphicsScreen.Scene.Children.Add(_particleSystemNode2);
}
/// <summary>
/// Diagonalizes the inertia matrix.
/// </summary>
/// <param name="inertia">The inertia matrix.</param>
/// <param name="inertiaDiagonal">The inertia of the principal axes.</param>
/// <param name="rotation">
/// The rotation that rotates from principal axis space to parent/world space.
/// </param>
/// <remarks>
/// All valid inertia matrices can be transformed into a coordinate space where all elements
/// non-diagonal matrix elements are 0. The axis of this special space are the principal axes.
/// </remarks>
internal static void DiagonalizeInertia(Matrix33F inertia, out Vector3F inertiaDiagonal, out Matrix33F rotation)
{
// Alternatively we could use Jacobi transformation (iterative method, see Bullet/btMatrix3x3.diagonalize()
// and Numerical Recipes book) or we could find the eigenvalues using the characteristic
// polynomial which is a cubic polynomial and then solve for the eigenvectors (see Numeric
// Recipes and "Mathematics for 3D Game Programming and Computer Graphics" chapter ray-tracing
// for cubic equations and computation of bounding boxes.
// Perform eigenvalue decomposition.
var eigenValueDecomposition = new EigenvalueDecompositionF(inertia.ToMatrixF());
inertiaDiagonal = eigenValueDecomposition.RealEigenvalues.ToVector3F();
rotation = eigenValueDecomposition.V.ToMatrix33F();
if (!rotation.IsRotation)
{
// V is orthogonal but not necessarily a rotation. If it is no rotation
// we have to swap two columns.
MathHelper.Swap(ref inertiaDiagonal.Y, ref inertiaDiagonal.Z);
Vector3F dummy = rotation.GetColumn(1);
rotation.SetColumn(1, rotation.GetColumn(2));
rotation.SetColumn(2, dummy);
Debug.Assert(rotation.IsRotation);
}
}
public void SerializationXml2()
{
Matrix33F m1 = new Matrix33F(12, 23, 45,
56, 67, 89,
90, 12, 43.3f);
Matrix33F m2;
string fileName = "SerializationMatrix33F_DataContractSerializer.xml";
if (File.Exists(fileName))
{
File.Delete(fileName);
}
var serializer = new DataContractSerializer(typeof(Matrix33F));
using (var stream = new FileStream(fileName, FileMode.CreateNew, FileAccess.Write))
using (var writer = XmlDictionaryWriter.CreateTextWriter(stream, Encoding.UTF8))
serializer.WriteObject(writer, m1);
using (var stream = new FileStream(fileName, FileMode.Open, FileAccess.Read))
using (var reader = XmlDictionaryReader.CreateTextReader(stream, new XmlDictionaryReaderQuotas()))
m2 = (Matrix33F)serializer.ReadObject(reader);
Assert.AreEqual(m1, m2);
}
private void SetCameraPose()
{
var vehiclePose = _vehicle.Pose;
if (_useSpectatorView)
{
// Spectator Mode:
// Camera is looking at the car from a fixed location in the level.
Vector3F position = new Vector3F(10, 8, 10);
Vector3F target = vehiclePose.Position;
Vector3F up = Vector3F.UnitY;
// Set the new camera view matrix. (Setting the View matrix changes the Pose.
// The pose is simply the inverse of the view matrix).
CameraNode.View = Matrix44F.CreateLookAt(position, target, up);
}
else
{
// Player Camera:
// Camera moves with the car. The look direction can be changed by moving the mouse.
Matrix33F yaw = Matrix33F.CreateRotationY(_yaw);
Matrix33F pitch = Matrix33F.CreateRotationX(_pitch);
Matrix33F orientation = vehiclePose.Orientation * yaw * pitch;
Vector3F forward = orientation * -Vector3F.UnitZ;
Vector3F up = Vector3F.UnitY;
Vector3F position = vehiclePose.Position - 10 * forward + 5 * up;
Vector3F target = vehiclePose.Position + 1 * up;
CameraNode.View = Matrix44F.CreateLookAt(position, target, up);
}
}
public void AbsoluteStatic()
{
Matrix33F m = new Matrix33F(-1, -2, -3, -4, -5, -6, -7, -8, -9);
Matrix33F absoluteM = Matrix33F.Absolute(m);
Assert.AreEqual(1, absoluteM.M00);
Assert.AreEqual(2, absoluteM.M01);
Assert.AreEqual(3, absoluteM.M02);
Assert.AreEqual(4, absoluteM.M10);
Assert.AreEqual(5, absoluteM.M11);
Assert.AreEqual(6, absoluteM.M12);
Assert.AreEqual(7, absoluteM.M20);
Assert.AreEqual(8, absoluteM.M21);
Assert.AreEqual(9, absoluteM.M22);
m = new Matrix33F(1, 2, 3, 4, 5, 6, 7, 8, 9);
absoluteM = Matrix33F.Absolute(m);
Assert.AreEqual(1, absoluteM.M00);
Assert.AreEqual(2, absoluteM.M01);
Assert.AreEqual(3, absoluteM.M02);
Assert.AreEqual(4, absoluteM.M10);
Assert.AreEqual(5, absoluteM.M11);
Assert.AreEqual(6, absoluteM.M12);
Assert.AreEqual(7, absoluteM.M20);
Assert.AreEqual(8, absoluteM.M21);
Assert.AreEqual(9, absoluteM.M22);
}
/// <summary>
/// Diagonalizes the inertia matrix.
/// </summary>
/// <param name="inertia">The inertia matrix.</param>
/// <param name="inertiaDiagonal">The inertia of the principal axes.</param>
/// <param name="rotation">
/// The rotation that rotates from principal axis space to parent/world space.
/// </param>
/// <remarks>
/// All valid inertia matrices can be transformed into a coordinate space where all elements
/// non-diagonal matrix elements are 0. The axis of this special space are the principal axes.
/// </remarks>
internal static void DiagonalizeInertia(Matrix33F inertia, out Vector3F inertiaDiagonal, out Matrix33F rotation)
{
// Alternatively we could use Jacobi transformation (iterative method, see Bullet/btMatrix3x3.diagonalize()
// and Numerical Recipes book) or we could find the eigenvalues using the characteristic
// polynomial which is a cubic polynomial and then solve for the eigenvectors (see Numeric
// Recipes and "Mathematics for 3D Game Programming and Computer Graphics" chapter ray-tracing
// for cubic equations and computation of bounding boxes.
// Perform eigenvalue decomposition.
var eigenValueDecomposition = new EigenvalueDecompositionF(inertia.ToMatrixF());
inertiaDiagonal = eigenValueDecomposition.RealEigenvalues.ToVector3F();
rotation = eigenValueDecomposition.V.ToMatrix33F();
if (!rotation.IsRotation)
{
// V is orthogonal but not necessarily a rotation. If it is no rotation
// we have to swap two columns.
MathHelper.Swap(ref inertiaDiagonal.Y, ref inertiaDiagonal.Z);
Vector3F dummy = rotation.GetColumn(1);
rotation.SetColumn(1, rotation.GetColumn(2));
rotation.SetColumn(2, dummy);
Debug.Assert(rotation.IsRotation);
}
}
public void Properties()
{
Matrix33F m = new Matrix33F(rowMajor, MatrixOrder.RowMajor);
Assert.AreEqual(1.0f, m.M00);
Assert.AreEqual(2.0f, m.M01);
Assert.AreEqual(3.0f, m.M02);
Assert.AreEqual(4.0f, m.M10);
Assert.AreEqual(5.0f, m.M11);
Assert.AreEqual(6.0f, m.M12);
Assert.AreEqual(7.0f, m.M20);
Assert.AreEqual(8.0f, m.M21);
Assert.AreEqual(9.0f, m.M22);
m = Matrix33F.Zero;
m.M00 = 1.0f;
m.M01 = 2.0f;
m.M02 = 3.0f;
m.M10 = 4.0f;
m.M11 = 5.0f;
m.M12 = 6.0f;
m.M20 = 7.0f;
m.M21 = 8.0f;
m.M22 = 9.0f;
Assert.AreEqual(new Matrix33F(rowMajor, MatrixOrder.RowMajor), m);
}
public SkeletonMappingSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Get dude model and start animation on the dude.
var modelNode = ContentManager.Load <ModelNode>("Dude/Dude");
_dudeMeshNode = modelNode.GetSubtree().OfType <MeshNode>().First().Clone();
_dudeMeshNode.PoseLocal = new Pose(new Vector3F(-0.5f, 0, 0), Matrix33F.CreateRotationY(ConstantsF.Pi));
SampleHelper.EnablePerPixelLighting(_dudeMeshNode);
GraphicsScreen.Scene.Children.Add(_dudeMeshNode);
var animations = _dudeMeshNode.Mesh.Animations;
var loopingAnimation = new AnimationClip <SkeletonPose>(animations.Values.First())
{
LoopBehavior = LoopBehavior.Cycle,
Duration = TimeSpan.MaxValue,
};
AnimationService.StartAnimation(loopingAnimation, (IAnimatableProperty)_dudeMeshNode.SkeletonPose);
// Get marine model - do not start any animations on the marine model.
modelNode = ContentManager.Load <ModelNode>("Marine/PlayerMarine");
_marineMeshNode = modelNode.GetSubtree().OfType <MeshNode>().First().Clone();
_marineMeshNode.PoseLocal = new Pose(new Vector3F(0.5f, 0, 0), Matrix33F.CreateRotationY(ConstantsF.Pi));
SampleHelper.EnablePerPixelLighting(_marineMeshNode);
GraphicsScreen.Scene.Children.Add(_marineMeshNode);
CreateSkeletonMapper();
}
//--------------------------------------------------------------
#region Creation & Cleanup
//--------------------------------------------------------------
/// <overloads>
/// <summary>
/// Initializes a new instance of the <see cref="TerrainDecalLayer"/> class.
/// </summary>
/// </overloads>
///
/// <summary>
/// Initializes a new instance of the <see cref="TerrainDecalLayer"/> class with the default
/// material.
/// </summary>
/// <param name="graphicService">The graphic service.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="graphicService"/> is <see langword="null"/>.
/// </exception>
public TerrainDecalLayer(IGraphicsService graphicService)
{
if (graphicService == null)
{
throw new ArgumentNullException("graphicService");
}
// Use a down orientation per default.
Pose = new Pose(Matrix33F.CreateRotationX(-ConstantsF.PiOver2));
var effect = graphicService.Content.Load <Effect>("DigitalRune/Terrain/TerrainDecalLayer");
Material = new Material
{
{ "Detail", new EffectBinding(graphicService, effect, null, EffectParameterHint.Material) }
};
FadeOutStart = int.MaxValue;
FadeOutEnd = int.MaxValue;
Width = 1;
Height = 1;
DiffuseColor = new Vector3F(1, 1, 1);
SpecularColor = new Vector3F(1, 1, 1);
SpecularPower = 10;
Alpha = 1;
DiffuseTexture = graphicService.GetDefaultTexture2DWhite();
SpecularTexture = graphicService.GetDefaultTexture2DBlack();
NormalTexture = graphicService.GetDefaultNormalTexture();
HeightTextureScale = 1;
HeightTexture = graphicService.GetDefaultTexture2DBlack();
UpdateAabb();
}
public void Constructors()
{
Matrix33F m = new Matrix33F(1.0f, 2.0f, 3.0f,
4.0f, 5.0f, 6.0f,
7.0f, 8.0f, 9.0f);
for (int i = 0; i < 9; i++)
{
Assert.AreEqual(rowMajor[i], m[i]);
}
m = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
for (int i = 0; i < 9; i++)
{
Assert.AreEqual(rowMajor[i], m[i]);
}
m = new Matrix33F(rowMajor, MatrixOrder.RowMajor);
for (int i = 0; i < 9; i++)
{
Assert.AreEqual(rowMajor[i], m[i]);
}
m = new Matrix33F(new List <float>(columnMajor), MatrixOrder.ColumnMajor);
for (int i = 0; i < 9; i++)
{
Assert.AreEqual(rowMajor[i], m[i]);
}
m = new Matrix33F(new List <float>(rowMajor), MatrixOrder.RowMajor);
for (int i = 0; i < 9; i++)
{
Assert.AreEqual(rowMajor[i], m[i]);
}
m = new Matrix33F(new float[3, 3] {
{ 1, 2, 3 },
{ 4, 5, 6 },
{ 7, 8, 9 }
});
for (int i = 0; i < 9; i++)
{
Assert.AreEqual(rowMajor[i], m[i]);
}
m = new Matrix33F(new float[3][] { new float[3] {
1, 2, 3
},
new float[3] {
4, 5, 6
},
new float[3] {
7, 8, 9
} });
for (int i = 0; i < 9; i++)
{
Assert.AreEqual(rowMajor[i], m[i]);
}
}
public SkinnedEffectSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
SampleFramework.IsMouseVisible = false;
GraphicsScreen.ClearBackground = true;
GraphicsScreen.BackgroundColor = Color.CornflowerBlue;
SetCamera(new Vector3F(1, 1, 3), 0.2f, 0);
// Create a sky mesh and add an instance of this mesh to the scene.
var skyMesh = ProceduralSkyDome.CreateMesh(GraphicsService, ContentManager.Load <Texture2D>("sky"));
_sky = new MeshNode(skyMesh);
_sky.Name = "Sky"; // Always set a name - very useful for debugging!
GraphicsScreen.Scene.Children.Add(_sky);
// Load the skinned model. This model is processed using the DigitalRune Model
// Processor - not the default XNA model processor!
// In the folder that contains dude.fbx, there are several XML files (*.drmdl and *.drmat)
// which define the materials of the model. These material description files are
// automatically processed by the DigitalRune Model Processor. Please browse
// to the content folder and have a look at the *.drmdl and *.drmat files.
var dudeModel = ContentManager.Load <ModelNode>("Dude/Dude");
dudeModel = dudeModel.Clone();
dudeModel.PoseWorld = new Pose(Matrix33F.CreateRotationY(ConstantsF.Pi));
GraphicsScreen.Scene.Children.Add(dudeModel);
// The dude model consists of a single mesh.
var dudeMeshNode = dudeModel.GetSubtree().OfType <MeshNode>().First();
var mesh = dudeMeshNode.Mesh;
/*
* // The dude mesh consists of different materials (head, eyes, torso, ...).
* // We could change some of the material properties...
* foreach (var material in mesh.Materials)
* {
* // Get all SkinnedEffectBindings which wrap the XNA SkinnedEffect.
* // A material can consist of several effects - one effect for each render pass.
* // (Per default there is only one render pass called "Default".)
* foreach (var effectBinding in material.EffectBindings.OfType<SkinnedEffectBinding>())
* {
* // We could change effect parameters here, for example:
* effectBinding.PreferPerPixelLighting = true;
* }
* }
*/
// The DigitalRune Model Processor also loads animations.
// Start the first animation of the dude and let it loop forever.
// (We keep the animation controller to be able to stop the animation in
// Dispose() below.)
var timeline = new TimelineClip(mesh.Animations.Values.First())
{
Duration = TimeSpan.MaxValue,
LoopBehavior = LoopBehavior.Cycle,
};
_animationController = AnimationService.StartAnimation(timeline, (IAnimatableProperty)dudeMeshNode.SkeletonPose);
}
public void GetColumn()
{
Matrix33F m = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
Assert.AreEqual(new Vector3F(1.0f, 4.0f, 7.0f), m.GetColumn(0));
Assert.AreEqual(new Vector3F(2.0f, 5.0f, 8.0f), m.GetColumn(1));
Assert.AreEqual(new Vector3F(3.0f, 6.0f, 9.0f), m.GetColumn(2));
}
/// <summary>
/// Initializes a new instance of the <see cref="CloudMapLayer" /> struct.
/// </summary>
/// <param name="texture">The <see cref="Texture" />.</param>
/// <param name="textureMatrix">The <see cref="TextureMatrix" />.</param>
/// <param name="densityOffset">The <see cref="DensityOffset" />.</param>
/// <param name="densityScale">The <see cref="DensityScale" />.</param>
/// <param name="animationSpeed">The <see cref="AnimationSpeed"/>.</param>
public CloudMapLayer(Texture2D texture, Matrix33F textureMatrix, float densityOffset, float densityScale, float animationSpeed)
{
Texture = texture;
TextureMatrix = textureMatrix;
DensityScale = densityScale;
DensityOffset = densityOffset;
AnimationSpeed = animationSpeed;
}
public void GetRow()
{
Matrix33F m = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
Assert.AreEqual(new Vector3F(1.0f, 2.0f, 3.0f), m.GetRow(0));
Assert.AreEqual(new Vector3F(4.0f, 5.0f, 6.0f), m.GetRow(1));
Assert.AreEqual(new Vector3F(7.0f, 8.0f, 9.0f), m.GetRow(2));
}
internal static Pose ToDigitalRune(this Matrix4x4 matrix)
{
var position = new Vector3F(matrix.M30, matrix.M31, matrix.M32);
var orientation = new Matrix33F(matrix.M00, matrix.M10, matrix.M20,
matrix.M01, matrix.M11, matrix.M21,
matrix.M02, matrix.M12, matrix.M22);
return new Pose(position, orientation);
}
public void Transposed()
{
Matrix33F m = new Matrix33F(rowMajor, MatrixOrder.RowMajor);
Matrix33F mt = new Matrix33F(rowMajor, MatrixOrder.ColumnMajor);
Assert.AreEqual(mt, m.Transposed);
Assert.AreEqual(Matrix33F.Identity, Matrix33F.Identity.Transposed);
}
public void CrossProductMatrix()
{
Vector3F v = new Vector3F(-1.0f, -2.0f, -3.0f);
Vector3F w = new Vector3F(4.0f, 5.0f, 6.0f);
Matrix33F m = v.ToCrossProductMatrix();
Assert.AreEqual(Vector3F.Cross(v, w), v.ToCrossProductMatrix() * w);
}
/// <summary>
/// Initializes a new instance of the <see cref="CloudMapLayer" /> struct.
/// </summary>
/// <param name="texture">The <see cref="Texture" />.</param>
/// <param name="textureMatrix">The <see cref="TextureMatrix" />.</param>
/// <param name="densityOffset">The <see cref="DensityOffset" />.</param>
/// <param name="densityScale">The <see cref="DensityScale" />.</param>
/// <param name="animationSpeed">The <see cref="AnimationSpeed"/>.</param>
public CloudMapLayer(Texture2D texture, Matrix33F textureMatrix, float densityOffset, float densityScale, float animationSpeed)
{
Texture = texture;
TextureMatrix = textureMatrix;
DensityScale = densityScale;
DensityOffset = densityOffset;
AnimationSpeed = animationSpeed;
}
/// <overloads>
/// <summary>
/// Computes the covariance matrix for a list of points.
/// </summary>
/// </overloads>
///
/// <summary>
/// Computes the covariance matrix for a list of 3-dimensional points (single-precision).
/// </summary>
/// <param name="points">The points.</param>
/// <returns>The covariance matrix.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="points"/> is <see langword="null"/>.
/// </exception>
public static Matrix33F ComputeCovarianceMatrix(IList<Vector3F> points)
{
// Notes: See "Real-Time Collision Detection" p. 93
if (points == null)
throw new ArgumentNullException("points");
int numberOfPoints = points.Count;
float oneOverNumberOfPoints = 1f / numberOfPoints;
// Compute the center of mass.
Vector3F centerOfMass = Vector3F.Zero;
for (int i = 0; i < numberOfPoints; i++)
centerOfMass += points[i];
centerOfMass *= oneOverNumberOfPoints;
// Compute covariance matrix.
float c00 = 0;
float c11 = 0;
float c22 = 0;
float c01 = 0;
float c02 = 0;
float c12 = 0;
for (int i = 0; i < numberOfPoints; i++)
{
// Translate points so that center of mass is at origin.
Vector3F p = points[i] - centerOfMass;
// Compute covariance of translated point.
c00 += p.X * p.X;
c11 += p.Y * p.Y;
c22 += p.Z * p.Z;
c01 += p.X * p.Y;
c02 += p.X * p.Z;
c12 += p.Y * p.Z;
}
c00 *= oneOverNumberOfPoints;
c11 *= oneOverNumberOfPoints;
c22 *= oneOverNumberOfPoints;
c01 *= oneOverNumberOfPoints;
c02 *= oneOverNumberOfPoints;
c12 *= oneOverNumberOfPoints;
Matrix33F covarianceMatrix = new Matrix33F(c00, c01, c02,
c01, c11, c12,
c02, c12, c22);
return covarianceMatrix;
}
/// <summary>
/// Renders a skybox.
/// </summary>
/// <param name="texture">The cube map with the sky texture.</param>
/// <param name="orientation">The orientation of the skybox.</param>
/// <param name="exposure">The exposure factor that is multiplied to the cube map values to change the brightness.
/// (Usually 1 or higher).</param>
/// <param name="context">
/// The render context. (<see cref="RenderContext.CameraNode"/> needs to be set.)
/// </param>
/// <exception cref="ArgumentNullException">
/// <paramref name="texture"/> or <paramref name="context"/> is <see langword="null"/>.
/// </exception>
public void Render(TextureCube texture, Matrix33F orientation, float exposure, RenderContext context)
{
if (texture == null)
throw new ArgumentNullException("texture");
if (context == null)
throw new ArgumentNullException("context");
context.Validate(_effect);
context.ThrowIfCameraMissing();
var graphicsDevice = context.GraphicsService.GraphicsDevice;
if (graphicsDevice.GraphicsProfile == GraphicsProfile.Reach)
RenderReach(texture, orientation, exposure, context);
else
RenderHiDef(texture, orientation, exposure, context);
}
//--------------------------------------------------------------
/// <summary>
/// Initializes a new instance of the <see cref="CloudMap" /> class.
/// </summary>
public LayeredCloudMap()
{
Coverage = 0.5f;
Density = 10;
Size = 1024;
Seed = 1234567;
// Initialize with 8 octaves of static noise.
Layers = new CloudMapLayer[8];
var scale = new Matrix33F(0.2f, 0, 0,
0, 0.2f, 0,
0, 0, 1);
Layers[0] = new CloudMapLayer(null, scale * new Matrix33F(1, 0, 0, 0, 1, 0, 0, 0, 1), -0.5f, 1.0f, 0);
Layers[1] = new CloudMapLayer(null, scale * new Matrix33F(2, 0, 0, 0, 2, 0, 0, 0, 1), -0.5f, 1.0f / 2.0f, 0);
Layers[2] = new CloudMapLayer(null, scale * new Matrix33F(4, 0, 0, 0, 4, 0, 0, 0, 1), -0.5f, 1.0f / 4.0f, 0);
Layers[3] = new CloudMapLayer(null, scale * new Matrix33F(8, 0, 0, 0, 8, 0, 0, 0, 1), -0.5f, 1.0f / 8.0f, 0);
Layers[4] = new CloudMapLayer(null, scale * new Matrix33F(16, 0, 0, 0, 16, 0, 0, 0, 1), -0.5f, 1.0f / 16.0f, 0);
Layers[5] = new CloudMapLayer(null, scale * new Matrix33F(32, 0, 0, 0, 32, 0, 0, 0, 1), -0.5f, 1.0f / 32.0f, 0);
Layers[6] = new CloudMapLayer(null, scale * new Matrix33F(64, 0, 0, 0, 64, 0, 0, 0, 1), -0.5f, 1.0f / 64.0f, 0);
Layers[7] = new CloudMapLayer(null, scale * new Matrix33F(128, 0, 0, 0, 128, 0, 0, 0, 1), -0.5f, 1.0f / 128.0f, 0);
}
public void MultiplyMatrixOperator()
{
Matrix33F m = new Matrix33F(12, 23, 45,
67, 89, 90,
43, 65, 87);
Assert.AreEqual(Matrix33F.Zero, m * Matrix33F.Zero);
Assert.AreEqual(Matrix33F.Zero, Matrix33F.Zero * m);
Assert.AreEqual(m, m * Matrix33F.Identity);
Assert.AreEqual(m, Matrix33F.Identity * m);
Assert.IsTrue(Matrix33F.AreNumericallyEqual(Matrix33F.Identity, m * m.Inverse));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(Matrix33F.Identity, m.Inverse * m));
Matrix33F m1 = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
Matrix33F m2 = new Matrix33F(12, 23, 45,
67, 89, 90,
43, 65, 87);
Matrix33F result = m1 * m2;
for (int column = 0; column < 3; column++)
for (int row = 0; row < 3; row++)
Assert.AreEqual(Vector3F.Dot(m1.GetRow(row), m2.GetColumn(column)), result[row, column]);
}
public void MultiplicationOperator()
{
float s = 0.1234f;
Matrix33F m = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
m = s * m;
for (int i = 0; i < 9; i++)
Assert.AreEqual(rowMajor[i] * s, m[i]);
m = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
m = m * s;
for (int i = 0; i < 9; i++)
Assert.AreEqual(rowMajor[i] * s, m[i]);
}
private static void TryToMakeIdentityMatrix(ref Matrix33F m)
{
// If the rotated space is parallel to the world space, then we want to use
// the identity matrix directly.
if (Numeric.AreEqual(1, Math.Abs(m.M00) + Math.Abs(m.M10) + Math.Abs(m.M20)) // Is the first column UnitX, UnitY or UnitZ?
&& Numeric.AreEqual(1, Math.Abs(m.M01) + Math.Abs(m.M11) + Math.Abs(m.M21))) // Is the second column UnitX, UnitY or UnitZ?
{
m = Matrix33F.Identity;
}
}
public static void GetMass(ITriangleMesh mesh, out float mass, out Vector3F centerOfMass, out Matrix33F inertia)
{
if (mesh == null)
throw new ArgumentNullException("mesh");
// Integral variables.
float i0 = 0, i1 = 0, i2 = 0, i3 = 0, i4 = 0, i5 = 0, i6 = 0, i7 = 0, i8 = 0, i9 = 0;
int numberOfTriangles = mesh.NumberOfTriangles;
for (int triangleIndex = 0; triangleIndex < numberOfTriangles; triangleIndex++)
{
var triangle = mesh.GetTriangle(triangleIndex);
// Vertex coordinates.
Vector3F v0 = triangle.Vertex0;
Vector3F v1 = triangle.Vertex1;
Vector3F v2 = triangle.Vertex2;
// Edges and cross products of edges
Vector3F a = v1 - v0;
Vector3F b = v2 - v0;
Vector3F d = Vector3F.Cross(a, b);
// Compute integral terms.
float f1x, f2x, f3x, g0x, g1x, g2x;
ComputePolyhedronMassSubExpressions(v0.X, v1.X, v2.X, out f1x, out f2x, out f3x, out g0x, out g1x, out g2x);
float f1y, f2y, f3y, g0y, g1y, g2y;
ComputePolyhedronMassSubExpressions(v0.Y, v1.Y, v2.Y, out f1y, out f2y, out f3y, out g0y, out g1y, out g2y);
float f1z, f2z, f3z, g0z, g1z, g2z;
ComputePolyhedronMassSubExpressions(v0.Z, v1.Z, v2.Z, out f1z, out f2z, out f3z, out g0z, out g1z, out g2z);
// Update integrals.
i0 += d.X * f1x;
i1 += d.X * f2x;
i2 += d.Y * f2y;
i3 += d.Z * f2z;
i4 += d.X * f3x;
i5 += d.Y * f3y;
i6 += d.Z * f3z;
i7 += d.X * (v0.Y * g0x + v1.Y * g1x + v2.Y * g2x);
i8 += d.Y * (v0.Z * g0y + v1.Z * g1y + v2.Z * g2y);
i9 += d.Z * (v0.X * g0z + v1.X * g1z + v2.X * g2z);
}
i0 /= 6.0f;
i1 /= 24.0f;
i2 /= 24.0f;
i3 /= 24.0f;
i4 /= 60.0f;
i5 /= 60.0f;
i6 /= 60.0f;
i7 /= 120.0f;
i8 /= 120.0f;
i9 /= 120.0f;
mass = i0;
centerOfMass = 1.0f / mass * new Vector3F(i1, i2, i3);
// Clamp to zero.
if (Numeric.IsZero(centerOfMass.X))
centerOfMass.X = 0;
if (Numeric.IsZero(centerOfMass.Y))
centerOfMass.Y = 0;
if (Numeric.IsZero(centerOfMass.Z))
centerOfMass.Z = 0;
// Inertia around the world origin.
inertia.M00 = i5 + i6;
inertia.M11 = i4 + i6;
inertia.M22 = i4 + i5;
inertia.M01 = inertia.M10 = Numeric.IsZero(i7) ? 0 : -i7;
inertia.M12 = inertia.M21 = Numeric.IsZero(i8) ? 0 : -i8;
inertia.M02 = inertia.M20 = Numeric.IsZero(i9) ? 0 : -i9;
// Inertia around center of mass.
inertia = GetUntranslatedMassInertia(mass, inertia, centerOfMass);
}
public void Addition()
{
Matrix33F m1 = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
Matrix33F m2 = Matrix33F.One;
Matrix33F result = Matrix33F.Add(m1, m2);
for (int i = 0; i < 9; i++)
Assert.AreEqual(rowMajor[i] + 1.0f, result[i]);
}
public void Properties()
{
Matrix33F m = new Matrix33F(rowMajor, MatrixOrder.RowMajor);
Assert.AreEqual(1.0f, m.M00);
Assert.AreEqual(2.0f, m.M01);
Assert.AreEqual(3.0f, m.M02);
Assert.AreEqual(4.0f, m.M10);
Assert.AreEqual(5.0f, m.M11);
Assert.AreEqual(6.0f, m.M12);
Assert.AreEqual(7.0f, m.M20);
Assert.AreEqual(8.0f, m.M21);
Assert.AreEqual(9.0f, m.M22);
m = Matrix33F.Zero;
m.M00 = 1.0f;
m.M01 = 2.0f;
m.M02 = 3.0f;
m.M10 = 4.0f;
m.M11 = 5.0f;
m.M12 = 6.0f;
m.M20 = 7.0f;
m.M21 = 8.0f;
m.M22 = 9.0f;
Assert.AreEqual(new Matrix33F(rowMajor, MatrixOrder.RowMajor), m);
}
public void InverseWithNearSingularMatrix()
{
Matrix33F m = new Matrix33F(0.0001f, 0, 0,
0, 0.0001f, 0,
0, 0, 0.0001f);
Vector3F v = Vector3F.One;
Vector3F w = m * v;
Assert.IsTrue(Vector3F.AreNumericallyEqual(v, m.Inverse * w));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(Matrix33F.Identity, m * m.Inverse));
}
public void Indexer2d()
{
Matrix33F m = new Matrix33F(rowMajor, MatrixOrder.RowMajor);
for (int column = 0; column < 3; column++)
for (int row = 0; row < 3; row++)
Assert.AreEqual(columnMajor[column * 3 + row], m[row, column]);
m = Matrix33F.Zero;
for (int column = 0; column < 3; column++)
for (int row = 0; row < 3; row++)
m[row, column] = (float) (row * 3 + column + 1);
Assert.AreEqual(new Matrix33F(rowMajor, MatrixOrder.RowMajor), m);
}
public void Indexer1dException4()
{
Matrix33F m = new Matrix33F();
float x = m[9];
}
public void Indexer1dException2()
{
Matrix33F m = new Matrix33F();
m[9] = 0.0f;
}
public void Indexer1d()
{
Matrix33F m = new Matrix33F(rowMajor, MatrixOrder.RowMajor);
for (int i = 0; i < 9; i++)
Assert.AreEqual(rowMajor[i], m[i]);
m = Matrix33F.Zero;
for (int i = 0; i < 9; i++)
m[i] = rowMajor[i];
Assert.AreEqual(new Matrix33F(rowMajor, MatrixOrder.RowMajor), m);
}
public void ImplicitMatrix33DCast()
{
float m00 = 23.5f; float m01 = 0.0f; float m02 = -11.0f;
float m10 = 33.5f; float m11 = 1.1f; float m12 = -12.0f;
float m20 = 43.5f; float m21 = 2.2f; float m22 = -13.0f;
Matrix33D matrix33D = new Matrix33F(m00, m01, m02, m10, m11, m12, m20, m21, m22);
Assert.IsTrue(Numeric.AreEqual(m00, (float)matrix33D[0, 0]));
Assert.IsTrue(Numeric.AreEqual(m01, (float)matrix33D[0, 1]));
Assert.IsTrue(Numeric.AreEqual(m02, (float)matrix33D[0, 2]));
Assert.IsTrue(Numeric.AreEqual(m10, (float)matrix33D[1, 0]));
Assert.IsTrue(Numeric.AreEqual(m11, (float)matrix33D[1, 1]));
Assert.IsTrue(Numeric.AreEqual(m12, (float)matrix33D[1, 2]));
Assert.IsTrue(Numeric.AreEqual(m20, (float)matrix33D[2, 0]));
Assert.IsTrue(Numeric.AreEqual(m21, (float)matrix33D[2, 1]));
Assert.IsTrue(Numeric.AreEqual(m22, (float)matrix33D[2, 2]));
}
public void MultiplyVectorOperator()
{
Vector3F v = new Vector3F(2.34f, 3.45f, 4.56f);
Assert.AreEqual(v, Matrix33F.Identity * v);
Assert.AreEqual(Vector3F.Zero, Matrix33F.Zero * v);
Matrix33F m = new Matrix33F(12, 23, 45,
67, 89, 90,
43, 65, 87);
Assert.IsTrue(Vector3F.AreNumericallyEqual(v, m * m.Inverse * v));
for (int i = 0; i < 3; i++)
Assert.AreEqual(Vector3F.Dot(m.GetRow(i), v), (m * v)[i]);
}
public void NegationOperator()
{
Matrix33F m = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
for (int i = 0; i < 9; i++)
Assert.AreEqual(-rowMajor[i], (-m)[i]);
}
public void Invert()
{
Assert.AreEqual(Matrix33F.Identity, Matrix33F.Identity.Inverse);
Matrix33F m = new Matrix33F(1, 2, 3,
2, 5, 8,
7, 6, -1);
Vector3F v = Vector3F.One;
Vector3F w = m * v;
Matrix33F im = m;
im.Invert();
Assert.IsTrue(Vector3F.AreNumericallyEqual(v, im * w));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(Matrix33F.Identity, m * im));
}
public void SerializationBinary()
{
Matrix33F m1 = new Matrix33F(12, 23, 45,
56, 67, 89,
90, 12, 43.3f);
Matrix33F m2;
string fileName = "SerializationMatrix33F.bin";
if (File.Exists(fileName))
File.Delete(fileName);
FileStream fs = new FileStream(fileName, FileMode.Create);
BinaryFormatter formatter = new BinaryFormatter();
formatter.Serialize(fs, m1);
fs.Close();
fs = new FileStream(fileName, FileMode.Open);
formatter = new BinaryFormatter();
m2 = (Matrix33F) formatter.Deserialize(fs);
fs.Close();
Assert.AreEqual(m1, m2);
}
public void InvertException()
{
Matrix33F m = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
m.Invert();
}
public void SerializationJson()
{
Matrix33F m1 = new Matrix33F(12, 23, 45,
56, 67, 89,
90, 12, 43.3f);
Matrix33F m2;
string fileName = "SerializationMatrix33F.json";
if (File.Exists(fileName))
File.Delete(fileName);
var serializer = new DataContractJsonSerializer(typeof(Matrix33F));
using (var stream = new FileStream(fileName, FileMode.CreateNew, FileAccess.Write))
serializer.WriteObject(stream, m1);
using (var stream = new FileStream(fileName, FileMode.Open, FileAccess.Read))
m2 = (Matrix33F)serializer.ReadObject(stream);
Assert.AreEqual(m1, m2);
}
public void IsNaN()
{
const int numberOfRows = 3;
const int numberOfColumns = 3;
Assert.IsFalse(new Matrix33F().IsNaN);
for (int r = 0; r < numberOfRows; r++)
{
for (int c = 0; c < numberOfColumns; c++)
{
Matrix33F m = new Matrix33F();
m[r, c] = float.NaN;
Assert.IsTrue(m.IsNaN);
}
}
}
// Computes a bounding box that contains the given points. The bounding box orientation
// is determined by the given orientation. The outputs are the box extent and the box center.
// volumeLimit is used for early out.
// Returns the volume of the new box.
// If the new box volume is smaller than volumeLimit, the 6 extreme vertices are moved to the
// front of the list.
private static float ComputeBoundingBox(IList<Vector3F> points, Matrix33F boxOrientation, float volumeLimit, out Vector3F boxExtent, out Vector3F boxCenter)
{
boxExtent = new Vector3F(float.PositiveInfinity);
boxCenter = Vector3F.Zero;
float volume = float.PositiveInfinity;
// The inverse orientation.
Matrix33F orientationInverse = boxOrientation.Transposed;
// The min and max points in the local box space.
Vector3F min = new Vector3F(float.PositiveInfinity);
Vector3F max = new Vector3F(float.NegativeInfinity);
// Remember the extreme points.
Vector3F minX = Vector3F.Zero;
Vector3F maxX = Vector3F.Zero;
Vector3F minY = Vector3F.Zero;
Vector3F maxY = Vector3F.Zero;
Vector3F minZ = Vector3F.Zero;
Vector3F maxZ = Vector3F.Zero;
for (int i = 0; i < points.Count; i++)
{
var point = points[i];
// Rotate point into local box space.
var localPoint = orientationInverse * point;
// Is this vertex on the box surface?
bool isExtreme = false;
// Store min and max coordinates.
if (localPoint.X < min.X)
{
min.X = localPoint.X;
minX = point;
isExtreme = true;
}
if (localPoint.X > max.X)
{
max.X = localPoint.X;
maxX = point;
isExtreme = true;
}
if (localPoint.Y < min.Y)
{
min.Y = localPoint.Y;
minY = point;
isExtreme = true;
}
if (localPoint.Y > max.Y)
{
max.Y = localPoint.Y;
maxY = point;
isExtreme = true;
}
if (localPoint.Z < min.Z)
{
min.Z = localPoint.Z;
minZ = point;
isExtreme = true;
}
if (localPoint.Z > max.Z)
{
max.Z = localPoint.Z;
maxZ = point;
isExtreme = true;
}
if (isExtreme)
{
// The box has grown.
boxExtent = max - min;
volume = boxExtent.X * boxExtent.Y * boxExtent.Z;
// Early out, if volume is beyond the limit.
if (volume > volumeLimit)
return float.PositiveInfinity;
}
}
// If we come to here, the new box is better than the old known box.
// Move the extreme vertices to the front of the list. This improves
// the early out drastically. - See Real-Time Collision Detection.
points.Remove(minX);
points.Insert(0, minX);
points.Remove(minY);
points.Insert(0, minY);
points.Remove(minZ);
points.Insert(0, minZ);
points.Remove(maxX);
points.Insert(0, maxX);
points.Remove(maxY);
points.Insert(0, maxY);
points.Remove(maxZ);
points.Insert(0, maxZ);
// Compute center and convert it to world space.
Vector3F localCenter = (min + max) / 2;
boxCenter = boxOrientation * localCenter;
return volume;
}
public void IsSymmetric()
{
Matrix33F m = new Matrix33F(new float[3, 3] { { 1, 2, 3 },
{ 2, 4, 5 },
{ 3, 5, 7 } });
Assert.AreEqual(true, m.IsSymmetric);
m = new Matrix33F(new float[3, 3] { { 1, 2, 3 },
{ 4, 5, 2 },
{ 7, 4, 1 } });
Assert.AreEqual(false, m.IsSymmetric);
}
/// <summary>
/// Gets the intersection point of three planes.
/// </summary>
/// <param name="planeA">The first plane.</param>
/// <param name="planeB">The second plane.</param>
/// <param name="planeC">The third plane.</param>
/// <returns>
/// The point that touches all three planes. (<see cref="float.NaN"/>, <see cref="float.NaN"/>,
/// <see cref="float.NaN"/>) is returned if there is no unique intersection point, for example,
/// when two planes are parallel or the planes intersect in a line.
/// </returns>
public static Vector3F GetIntersection(Plane planeA, Plane planeB, Plane planeC)
{
// Get a point that meets this requirements: Dot(plane.Normal, point) == plane.DistanceFromOrigin
Matrix33F matrix = new Matrix33F(planeA.Normal.X, planeA.Normal.Y, planeA.Normal.Z,
planeB.Normal.X, planeB.Normal.Y, planeB.Normal.Z,
planeC.Normal.X, planeC.Normal.Y, planeC.Normal.Z);
Vector3F distances = new Vector3F(planeA.DistanceFromOrigin, planeB.DistanceFromOrigin, planeC.DistanceFromOrigin);
bool isInvertible = matrix.TryInvert();
if (isInvertible)
return matrix * distances;
else
return new Vector3F(float.NaN);
}
public void Multiplication()
{
float s = 0.1234f;
Matrix33F m = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
m = Matrix33F.Multiply(s, m);
for (int i = 0; i < 9; i++)
Assert.AreEqual(rowMajor[i] * s, m[i]);
}
