/// <inheritdoc/>
public override Aabb GetAabb(Vector3F scale, Pose pose)
{
Vector3F halfExtent = new Vector3F(_widthX / 2, _widthY / 2, 0) * Vector3F.Absolute(scale);
// 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/>
public override Aabb GetAabb(Vector3F scale, Pose pose)
{
if (scale.X == scale.Y && scale.Y == scale.Z)
{
// 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 = 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));
}
}
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));
}
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]);
}
}
}
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
private void UpdateAabb()
{
// Note: This code was copied from BoxShape. See BoxShape.cs for detailed comments.
Vector3F halfExtent = new Vector3F(Width / 2, Height / 2, 0);
Matrix33F rotationMatrix = _pose.Orientation;
Vector3F worldX = rotationMatrix.GetRow(0);
Vector3F worldY = rotationMatrix.GetRow(1);
Vector3F worldZ = rotationMatrix.GetRow(2);
worldX = Vector3F.Absolute(worldX);
worldY = Vector3F.Absolute(worldY);
worldZ = Vector3F.Absolute(worldZ);
Vector3F halfExtentWorld = new Vector3F(Vector3F.Dot(halfExtent, worldX),
Vector3F.Dot(halfExtent, worldY),
Vector3F.Dot(halfExtent, worldZ));
Aabb = new Aabb(_pose.Position - halfExtentWorld, _pose.Position + halfExtentWorld);
}
public void SetRow()
{
Matrix33F m = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
m.SetRow(0, new Vector3F(0.1f, 0.2f, 0.3f));
Assert.AreEqual(new Vector3F(0.1f, 0.2f, 0.3f), 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));
m.SetRow(1, new Vector3F(0.4f, 0.5f, 0.6f));
Assert.AreEqual(new Vector3F(0.1f, 0.2f, 0.3f), m.GetRow(0));
Assert.AreEqual(new Vector3F(0.4f, 0.5f, 0.6f), m.GetRow(1));
Assert.AreEqual(new Vector3F(7.0f, 8.0f, 9.0f), m.GetRow(2));
m.SetRow(2, new Vector3F(0.7f, 0.8f, 0.9f));
Assert.AreEqual(new Vector3F(0.1f, 0.2f, 0.3f), m.GetRow(0));
Assert.AreEqual(new Vector3F(0.4f, 0.5f, 0.6f), m.GetRow(1));
Assert.AreEqual(new Vector3F(0.7f, 0.8f, 0.9f), m.GetRow(2));
}
public void MultiplyVector()
{
Vector3F v = new Vector3F(2.34f, 3.45f, 4.56f);
Assert.AreEqual(v, Matrix33F.Multiply(Matrix33F.Identity, v));
Assert.AreEqual(Vector3F.Zero, Matrix33F.Multiply(Matrix33F.Zero, v));
Matrix33F m = new Matrix33F(12, 23, 45,
67, 89, 90,
43, 65, 87);
Assert.IsTrue(Vector3F.AreNumericallyEqual(v, Matrix33F.Multiply(m * m.Inverse, v)));
for (int i = 0; i < 3; i++)
{
Assert.AreEqual(Vector3F.Dot(m.GetRow(i), v), Matrix33F.Multiply(m, v)[i]);
}
}
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 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 GetRowException2()
{
Matrix33F m = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
m.GetRow(3);
}
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));
}
public void SetRow()
{
Matrix33F m = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
m.SetRow(0, new Vector3F(0.1f, 0.2f, 0.3f));
Assert.AreEqual(new Vector3F(0.1f, 0.2f, 0.3f), 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));
m.SetRow(1, new Vector3F(0.4f, 0.5f, 0.6f));
Assert.AreEqual(new Vector3F(0.1f, 0.2f, 0.3f), m.GetRow(0));
Assert.AreEqual(new Vector3F(0.4f, 0.5f, 0.6f), m.GetRow(1));
Assert.AreEqual(new Vector3F(7.0f, 8.0f, 9.0f), m.GetRow(2));
m.SetRow(2, new Vector3F(0.7f, 0.8f, 0.9f));
Assert.AreEqual(new Vector3F(0.1f, 0.2f, 0.3f), m.GetRow(0));
Assert.AreEqual(new Vector3F(0.4f, 0.5f, 0.6f), m.GetRow(1));
Assert.AreEqual(new Vector3F(0.7f, 0.8f, 0.9f), m.GetRow(2));
}
//--------------------------------------------------------------
#region Fields
//--------------------------------------------------------------
#endregion
//--------------------------------------------------------------
#region Properties & Events
//--------------------------------------------------------------
#endregion
//--------------------------------------------------------------
#region Creation & Cleanup
//--------------------------------------------------------------
#endregion
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
/// <summary>
/// Computes the axis-aligned bounding box (AABB) for this shape positioned in world space using
/// the given scale and <see cref="Pose"/>.
/// </summary>
/// <param name="scale">
/// The scale factor by which the shape should be scaled. The scaling is applied in the shape's
/// local space before the pose is applied.
/// </param>
/// <param name="pose">
/// The <see cref="Pose"/> of the shape. This pose defines how the shape should be positioned in
/// world space.
/// </param>
/// <returns>The AABB of the shape positioned in world space.</returns>
/// <remarks>
/// <para>
/// The AABB is axis-aligned to the axes of the world space (or the parent coordinate space).
/// </para>
/// <para>
/// The default implementation in <see cref="ConvexShape"/> uses the support mapping to compute
/// the AABB. Often the AABB can be computed more efficiently; in such cases this method should
/// be overridden in derived classes.
/// </para>
/// </remarks>
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 all axes.
Vector3F minimum = new Vector3F(
Vector3F.Dot(GetSupportPointNormalized(-worldX), worldX),
Vector3F.Dot(GetSupportPointNormalized(-worldY), worldY),
Vector3F.Dot(GetSupportPointNormalized(-worldZ), worldZ));
minimum = minimum * scale.X + pose.Position;
Vector3F maximum = new Vector3F(
Vector3F.Dot(GetSupportPointNormalized(worldX), worldX),
Vector3F.Dot(GetSupportPointNormalized(worldY), worldY),
Vector3F.Dot(GetSupportPointNormalized(worldZ), worldZ));
maximum = maximum * scale.X + pose.Position;
// Check minimum and maximum because scaling could be negative.
if (minimum <= maximum)
{
return(new Aabb(minimum, maximum));
}
else
{
return(new Aabb(maximum, minimum));
}
}
else
{
// Non-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 all axes.
Vector3F minimum = new Vector3F(
Vector3F.Dot(GetSupportPoint(-worldX, scale), worldX),
Vector3F.Dot(GetSupportPoint(-worldY, scale), worldY),
Vector3F.Dot(GetSupportPoint(-worldZ, scale), worldZ));
Vector3F maximum = new Vector3F(
Vector3F.Dot(GetSupportPoint(worldX, scale), worldX),
Vector3F.Dot(GetSupportPoint(worldY, scale), worldY),
Vector3F.Dot(GetSupportPoint(worldZ, scale), worldZ));
minimum += pose.Position;
maximum += pose.Position;
// Component-wise check minimum and maximum because scaling could be negative!
if (minimum.X > maximum.X)
{
MathHelper.Swap(ref minimum.X, ref maximum.X);
}
if (minimum.Y > maximum.Y)
{
MathHelper.Swap(ref minimum.Y, ref maximum.Y);
}
if (minimum.Z > maximum.Z)
{
MathHelper.Swap(ref minimum.Z, ref maximum.Z);
}
Debug.Assert(minimum <= maximum);
return(new Aabb(minimum, maximum));
}
}
public void GetRowException2()
{
Matrix33F m = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
m.GetRow(3);
}
