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 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);
}
public void GetScreenSizeWithPerspective()
{
// Camera
var projection = new PerspectiveProjection();
projection.SetFieldOfView(MathHelper.ToRadians(90), 2.0f / 1.0f, 1.0f, 100f);
var camera = new Camera(projection);
var cameraNode = new CameraNode(camera);
cameraNode.PoseWorld = new Pose(new Vector3F(123, 456, -789), Matrix33F.CreateRotation(new Vector3F(1, -2, 3), MathHelper.ToRadians(75)));
// 2:1 viewport
var viewport = new Viewport(10, 10, 200, 100);
// Test object
var shape = new SphereShape();
var geometricObject = new GeometricObject(shape);
// Empty sphere at camera position.
shape.Radius = 0;
geometricObject.Pose = cameraNode.PoseWorld;
Vector2F screenSize = GraphicsHelper.GetScreenSize(cameraNode, viewport, geometricObject);
Assert.AreEqual(0, screenSize.X);
Assert.AreEqual(0, screenSize.Y);
// Empty sphere centered at near plane.
shape.Radius = 0;
geometricObject.Pose = cameraNode.PoseWorld * new Pose(new Vector3F(0.123f, -0.543f, -1));
screenSize = GraphicsHelper.GetScreenSize(cameraNode, viewport, geometricObject);
Assert.AreEqual(0, screenSize.X);
Assert.AreEqual(0, screenSize.Y);
// Create sphere which as a bounding sphere of ~1 unit diameter:
// Since the bounding sphere is based on the AABB, we need to make the
// actual sphere a bit smaller.
shape.Radius = 1 / (2 * (float)Math.Sqrt(3)) + Numeric.EpsilonF;
// Sphere at camera position.
geometricObject.Pose = cameraNode.PoseWorld;
screenSize = GraphicsHelper.GetScreenSize(cameraNode, viewport, geometricObject);
Assert.Greater(screenSize.X, 200);
Assert.Greater(screenSize.Y, 100);
// Sphere at near plane.
geometricObject.Pose = cameraNode.PoseWorld * new Pose(new Vector3F(0.123f, -0.543f, -1));
screenSize = GraphicsHelper.GetScreenSize(cameraNode, viewport, geometricObject);
Assert.IsTrue(Numeric.AreEqual(screenSize.X, 50.0f, 10f));
Assert.IsTrue(Numeric.AreEqual(screenSize.Y, 50.0f, 10f));
// Double distance --> half size
geometricObject.Pose = cameraNode.PoseWorld * new Pose(new Vector3F(0.123f, -0.543f, -2));
screenSize = GraphicsHelper.GetScreenSize(cameraNode, viewport, geometricObject);
Assert.IsTrue(Numeric.AreEqual(screenSize.X, 25.0f, 5f));
Assert.IsTrue(Numeric.AreEqual(screenSize.Y, 25.0f, 5f));
}
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));
}
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));
}
public ConvexHullSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
SampleFramework.IsMouseVisible = false;
GraphicsScreen.ClearBackground = true;
GraphicsScreen.BackgroundColor = Color.CornflowerBlue;
SetCamera(new Vector3F(0, 1, 10), 0, 0);
// Generate random points.
var points = new List <Vector3F>();
for (int i = 0; i < 100; i++)
{
points.Add(RandomHelper.Random.NextVector3F(-1, 1));
}
// Apply random transformation to points to make this sample more interesting.
Matrix44F transform = new Matrix44F(
Matrix33F.CreateRotation(RandomHelper.Random.NextQuaternionF()) * Matrix33F.CreateScale(RandomHelper.Random.NextVector3F(0.1f, 2f)),
RandomHelper.Random.NextVector3F(-1, 1));
for (int i = 0; i < points.Count; i++)
{
points[i] = transform.TransformPosition(points[i]);
}
// Compute convex hull. The result is the mesh of the hull represented as a
// Doubly-Connected Edge List (DCEL).
DcelMesh convexHull = GeometryHelper.CreateConvexHull(points);
// We don't need the DCEL representation. Let's store the hull as a simpler triangle mesh.
TriangleMesh convexHullMesh = convexHull.ToTriangleMesh();
// Compute a tight-fitting oriented bounding box.
Vector3F boundingBoxExtent; // The bounding box dimensions (widths in X, Y and Z).
Pose boundingBoxPose; // The pose (world space position and orientation) of the bounding box.
GeometryHelper.ComputeBoundingBox(points, out boundingBoxExtent, out boundingBoxPose);
// (Note: The GeometryHelper also contains methods to compute a bounding sphere.)
var debugRenderer = GraphicsScreen.DebugRenderer;
foreach (var point in points)
{
debugRenderer.DrawPoint(point, Color.White, true);
}
debugRenderer.DrawShape(new TriangleMeshShape(convexHullMesh), Pose.Identity, Vector3F.One, Color.Violet, false, false);
debugRenderer.DrawBox(boundingBoxExtent.X, boundingBoxExtent.Y, boundingBoxExtent.Z, boundingBoxPose, Color.Red, true, false);
}
public void CreateRotation()
{
Matrix33F m = Matrix33F.CreateRotation(Vector3F.UnitX, 0.0f);
Assert.AreEqual(Matrix33F.Identity, m);
m = Matrix33F.CreateRotation(Vector3F.UnitX, (float)Math.PI / 2);
Assert.IsTrue(Vector3F.AreNumericallyEqual(Vector3F.UnitZ, m * Vector3F.UnitY));
m = Matrix33F.CreateRotation(Vector3F.UnitY, (float)Math.PI / 2);
Assert.IsTrue(Vector3F.AreNumericallyEqual(Vector3F.UnitX, m * Vector3F.UnitZ));
m = Matrix33F.CreateRotation(Vector3F.UnitZ, (float)Math.PI / 2);
Assert.IsTrue(Vector3F.AreNumericallyEqual(Vector3F.UnitY, m * Vector3F.UnitX));
}
public void ToLocal()
{
Vector3F startPoint = new Vector3F(10, 20, -40);
Vector3F endPoint = new Vector3F(-22, 34, 45);
Ray ray = new Ray(startPoint, (endPoint - startPoint).Normalized, (endPoint - startPoint).Length);
Pose pose = new Pose(new Vector3F(-5, 100, -20), Matrix33F.CreateRotation(new Vector3F(1, 2, 3), 0.123f));
startPoint = pose.ToLocalPosition(startPoint);
endPoint = pose.ToLocalPosition(endPoint);
ray.ToLocal(ref pose);
Assert.IsTrue(Vector3F.AreNumericallyEqual(startPoint, ray.Origin));
Assert.IsTrue(Vector3F.AreNumericallyEqual(endPoint, ray.Origin + ray.Direction * ray.Length));
}
private void UpdateEphemeris()
{
#if XBOX
_ephemeris.Time = new DateTimeOffset(_time.Ticks, TimeSpan.Zero);
#else
_ephemeris.Time = _time;
#endif
_ephemeris.Update();
var sunDirection = (Vector3F)_ephemeris.SunDirectionRefracted;
var sunUp = sunDirection.Orthonormal1;
var moonDirection = (Vector3F)_ephemeris.MoonPosition.Normalized;
var moonUp = (Vector3F)_ephemeris.EquatorialToWorld.TransformDirection(Vector3D.Up);
#if true
_starfield.PoseWorld = new Pose((Matrix33F)_ephemeris.EquatorialToWorld.Minor);
_sun.LookAt((Vector3F)_ephemeris.SunDirectionRefracted, sunUp);
_moon.SunDirection = (Vector3F)_ephemeris.SunPosition.Normalized;
#else
Vector3F sunRotationAxis = new Vector3F(0, -0.1f, 1).Normalized;
float hour = (float)_time.TimeOfDay.TotalHours / 24;
Matrix33F sunRotation = Matrix33F.CreateRotation(sunRotationAxis, hour * ConstantsF.TwoPi - ConstantsF.PiOver2);
_starfield.Orientation = sunRotation;
_sun.Direction = sunRotation * new Vector3F(1, 0, 0);
_moon.SunDirection = _sun.Direction;
#endif
_milkyWaySkybox.PoseWorld = new Pose(
(Matrix33F)_ephemeris.EquatorialToWorld.Minor
* Matrix33F.CreateRotationZ(ConstantsF.PiOver2)
* Matrix33F.CreateRotationX(ConstantsF.PiOver2));
_moon.LookAt(moonDirection, moonUp);
_cieSkyFilter.SunDirection = sunDirection;
_gradientSky.SunDirection = sunDirection;
_gradientTextureSky.SunDirection = sunDirection;
_gradientTextureSky.TimeOfDay = _time.TimeOfDay;
_scatteringSky.SunDirection = sunDirection;
_cloudLayerNode.SunDirection = _scatteringSky.SunDirection;
_cloudLayerNode.SunLight = ChangeSaturation(_scatteringSky.GetSunlight() / 5f, 1);
//_cloudPlaneRenderer.Color = new Vector4F(ChangeSaturation(_scatteringSky.GetFogColor(128), 0.9f) * 1.0f, 1);
//Vector3F c = (_scatteringSky.GetFogColor(128) + _scatteringSky.GetSunlight() / 10) / 2;
//_cloudPlaneRenderer.Color = new Vector4F(c, 1);
_cloudLayerNode.AmbientLight = _scatteringSky.GetAmbientLight(1024) / 6f;
//_cloudLayerNode.AmbientLight = _scatteringSky.GetFogColor(128) * _scatteringSky.GetAmbientLight(256).Length / 6f;
}
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)));
}
// 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 ToLocal()
{
Vector3F point0 = new Vector3F(10, 20, -40);
Vector3F point1 = new Vector3F(-22, 34, 45);
Line line = new Line(point0, (point1 - point0).Normalized);
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);
line.ToLocal(ref pose);
Vector3F dummy;
Assert.IsTrue(GeometryHelper.GetClosestPoint(line, point0, out dummy));
Assert.IsTrue(GeometryHelper.GetClosestPoint(line, point1, out dummy));
}
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 Test1()
{
Pose p = Pose.Identity;
Assert.AreEqual(Matrix44F.Identity, p.ToMatrix44F());
Assert.AreEqual(Matrix33F.Identity, p.Orientation);
Assert.AreEqual(Vector3F.Zero, p.Position);
p.Position = new Vector3F(1, 2, 3);
p.Orientation = Matrix33F.CreateRotation(new Vector3F(3, -4, 9), 0.49f);
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldDirection(Vector3F.UnitX), 0), p * new Vector4F(1, 0, 0, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldDirection(Vector3F.UnitY), 0), p * new Vector4F(0, 1, 0, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldDirection(Vector3F.UnitZ), 0), p * new Vector4F(0, 0, 1, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldPosition(Vector3F.UnitX), 1), p * new Vector4F(1, 0, 0, 1)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldPosition(Vector3F.UnitY), 1), p * new Vector4F(0, 1, 0, 1)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldPosition(Vector3F.UnitZ), 1), p * new Vector4F(0, 0, 1, 1)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalDirection(Vector3F.UnitX), 0), p.Inverse * new Vector4F(1, 0, 0, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalDirection(Vector3F.UnitY), 0), p.Inverse * new Vector4F(0, 1, 0, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalDirection(Vector3F.UnitZ), 0), p.Inverse * new Vector4F(0, 0, 1, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalPosition(Vector3F.UnitX), 1), p.Inverse * new Vector4F(1, 0, 0, 1)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalPosition(Vector3F.UnitY), 1), p.Inverse * new Vector4F(0, 1, 0, 1)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalPosition(Vector3F.UnitZ), 1), p.Inverse * new Vector4F(0, 0, 1, 1)));
Pose p2 = Pose.FromMatrix(new Matrix44F(p.Orientation, Vector3F.Zero));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(p.Orientation, p2.Orientation));
Assert.IsTrue(Vector3F.AreNumericallyEqual(p2.Position, Vector3F.Zero));
Matrix44F m = p2;
m.SetColumn(3, new Vector4F(p.Position, 1));
p2 = Pose.FromMatrix(m);
Assert.IsTrue(Matrix33F.AreNumericallyEqual(p.Orientation, p2.Orientation));
Assert.AreEqual(p.Position, p2.Position);
//Assert.IsTrue(Vector3F.AreNumericallyEqual(p.Position, p2.Position));
// Test other constructors.
Assert.AreEqual(Vector3F.Zero, new Pose(QuaternionF.CreateRotationX(0.3f)).Position);
Assert.AreEqual(Matrix33F.CreateRotationX(0.3f), new Pose(Matrix33F.CreateRotationX(0.3f)).Orientation);
Assert.AreEqual(new Vector3F(1, 2, 3), new Pose(new Vector3F(1, 2, 3)).Position);
Assert.AreEqual(Matrix33F.Identity, new Pose(new Vector3F(1, 2, 3)).Orientation);
}
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 override void Update(GameTime gameTime)
{
var debugRenderer = GraphicsScreen.DebugRenderer;
var cd = new CollisionDetection();
float sizeB = _part2.GeometricObject.Aabb.Extent.Length;
var perturbationEpsilon = sizeB * Math.Max(0.001f / 10, Numeric.EpsilonF * 10);
var perturbationAngle = perturbationEpsilon / sizeB;
Vector3F v;
v = new Vector3F(1, 0, 0);
var translation = v * perturbationEpsilon;
var rotation = Matrix33F.CreateRotation(v, perturbationAngle);
var poseB = _part2.GeometricObject.Pose;
var origPose = poseB;
poseB.Position = translation + poseB.Position;
poseB.Orientation = rotation * poseB.Orientation;
//((GeometricObject)_part2.GeometricObject).Pose = poseB;
var cp1 = cd.GetClosestPoints(_part1A, _part2);
//var cp2 = cd.GetClosestPoints(_part1B, _part2);
((GeometricObject)_part2.GeometricObject).Pose = origPose;
debugRenderer.Clear();
debugRenderer.DrawObject(_part1A.GeometricObject, Color.DarkGreen, true, false);
//debugRenderer.DrawObject(_part1B.GeometricObject, Color.DarkBlue, false, false);
debugRenderer.DrawObject(_part2.GeometricObject, Color.DarkViolet, true, false);
debugRenderer.DrawContact(cp1[0], 0.1f, Color.Yellow, true);
//debugRenderer.DrawContact(cp2[0], 0.1f, Color.Pink, true);
}
public override void Update(GameTime gameTime)
{
// Reflect velocity if objects collide:
// The collision domain contains a ContactSet for each pair of touching objects.
foreach (var contactSet in _domain.ContactSets)
{
// Get the touching objects.
var objectA = (MovingGeometricObject)contactSet.ObjectA.GeometricObject;
var objectB = (MovingGeometricObject)contactSet.ObjectB.GeometricObject;
// In rare cases, the collision detection cannot compute a contact because of
// numerical problems. Ignore this case, we usually get a contact in the next frame.
if (contactSet.Count == 0)
{
continue;
}
// Get the contact normal of the first collision point.
var contact = contactSet[0];
Vector3F normal = contact.Normal;
// Check if the objects move towards or away from each other in the direction of the normal.
if (Vector3F.Dot(objectB.LinearVelocity - objectA.LinearVelocity, normal) <= 0)
{
// Objects move towards each other. --> Reflect their velocities.
objectA.LinearVelocity -= 2 * Vector3F.ProjectTo(objectA.LinearVelocity, normal);
objectB.LinearVelocity -= 2 * Vector3F.ProjectTo(objectB.LinearVelocity, normal);
objectA.AngularVelocity = -objectA.AngularVelocity;
objectB.AngularVelocity = -objectB.AngularVelocity;
}
}
// Get the size of the current time step.
float timeStep = (float)gameTime.ElapsedGameTime.TotalSeconds;
// Move objects.
var objects = _domain.CollisionObjects.Select(co => co.GeometricObject).OfType <MovingGeometricObject>();
foreach (var obj in objects)
{
// Update position.
Vector3F position = obj.Pose.Position + obj.LinearVelocity * timeStep;
// Update rotation.
Vector3F rotationAxis = obj.AngularVelocity;
float angularSpeed = obj.AngularVelocity.Length;
Matrix33F rotation = (Numeric.IsZero(angularSpeed))
? Matrix33F.Identity
: Matrix33F.CreateRotation(rotationAxis, angularSpeed * timeStep);
var orientation = rotation * obj.Pose.Orientation;
// Incrementally updating the rotation matrix will eventually create a
// matrix which is not a rotation matrix anymore because of numerical
// problems. Re-othogonalization make sure that the matrix represents a
// rotation.
orientation.Orthogonalize();
obj.Pose = new Pose(position, orientation);
}
// Update collision domain. This computes new contact information.
_domain.Update(timeStep);
// Record some statistics.
int numberOfObjects = _domain.CollisionObjects.Count;
Profiler.AddValue("NumObjects", numberOfObjects);
// If there are n objects, we can have max. n * (n - 1) / 2 collisions.
Profiler.AddValue("NumObjectPairs", numberOfObjects * (numberOfObjects - 1f) / 2f);
// The first part of the collision detection is the "broad-phase" which
// filters out objects that cannot collide (e.g. using a fast bounding box test).
Profiler.AddValue("BroadPhasePairs", _domain.NumberOfBroadPhaseOverlaps);
// Finally, the collision detection computes the exact contact information and creates
// a ContactSet with the Contacts for each pair of colliding objects.
Profiler.AddValue("ContactSetCount", _domain.ContactSets.Count);
// Draw objects using the DebugRenderer of the graphics screen.
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
foreach (var collisionObject in _domain.CollisionObjects)
{
debugRenderer.DrawObject(collisionObject.GeometricObject, GraphicsHelper.GetUniqueColor(collisionObject), false, false);
}
}
public void CreateRotation()
{
QuaternionF q;
// From matrix vs. from angle/axis
Matrix33F m = Matrix33F.CreateRotation(Vector3F.UnitX, (float)Math.PI / 4);
q = QuaternionF.CreateRotation(m);
QuaternionF q2 = QuaternionF.CreateRotation(Vector3F.UnitX, (float)Math.PI / 4);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(q2, q));
m = Matrix33F.CreateRotation(Vector3F.UnitY, (float)Math.PI / 4);
q = QuaternionF.CreateRotation(m);
q2 = QuaternionF.CreateRotation(Vector3F.UnitY, (float)Math.PI / 4);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(q2, q));
m = Matrix33F.CreateRotation(Vector3F.UnitZ, (float)Math.PI / 4);
q = QuaternionF.CreateRotation(m);
q2 = QuaternionF.CreateRotation(Vector3F.UnitZ, (float)Math.PI / 4);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(q2, q));
// From vector-vector
Vector3F start, end;
start = Vector3F.UnitX;
end = Vector3F.UnitY;
q = QuaternionF.CreateRotation(start, end);
Assert.IsTrue(Vector3F.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = Vector3F.UnitY;
end = Vector3F.UnitZ;
q = QuaternionF.CreateRotation(start, end);
Assert.IsTrue(Vector3F.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = Vector3F.UnitZ;
end = Vector3F.UnitX;
q = QuaternionF.CreateRotation(start, end);
Assert.IsTrue(Vector3F.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = new Vector3F(1, 1, 1);
end = new Vector3F(1, 1, 1);
q = QuaternionF.CreateRotation(start, end);
Assert.IsTrue(Vector3F.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = new Vector3F(1.0f, 1.0f, 1.0f);
end = new Vector3F(-1.0f, -1.0f, -1.0f);
q = QuaternionF.CreateRotation(start, end);
Assert.IsTrue(Vector3F.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = new Vector3F(-1.0f, 2.0f, 1.0f);
end = new Vector3F(-2.0f, -1.0f, -1.0f);
q = QuaternionF.CreateRotation(start, end);
Assert.IsTrue(Vector3F.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
float degree45 = MathHelper.ToRadians(45);
q = QuaternionF.CreateRotation(degree45, Vector3F.UnitZ, degree45, Vector3F.UnitY, degree45, Vector3F.UnitX, false);
QuaternionF expected = QuaternionF.CreateRotation(Vector3F.UnitZ, degree45) * QuaternionF.CreateRotation(Vector3F.UnitY, degree45)
* QuaternionF.CreateRotation(Vector3F.UnitX, degree45);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(expected, q));
q = QuaternionF.CreateRotation(degree45, Vector3F.UnitZ, degree45, Vector3F.UnitY, degree45, Vector3F.UnitX, true);
expected = QuaternionF.CreateRotation(Vector3F.UnitX, degree45) * QuaternionF.CreateRotation(Vector3F.UnitY, degree45)
* QuaternionF.CreateRotation(Vector3F.UnitZ, degree45);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(expected, q));
}
public void CreateRotationException()
{
Matrix33F.CreateRotation(Vector3F.Zero, 1f);
}
// Render outline of sphere in perspective projection.
private void RenderSphereOutline(float radius, ref Vector3F center, ref Pose cameraPose, ref Vector3F right, ref Color color)
{
// TODO: Try alternative algorithm. See http://www.gamasutra.com/view/feature/131351/the_mechanics_of_robust_stencil_.php?page=6
// Forward direction of camera in world space.
Vector3F forward = cameraPose.Orientation * Vector3F.Forward;
// Vector pointing from camera position to center of sphere.
Vector3F cameraToCenter = center - cameraPose.Position;
float cameraToCenterLength = cameraToCenter.Length;
if (Numeric.IsLessOrEqual(cameraToCenterLength, radius))
{
// Camera is within sphere.
return;
}
cameraToCenter /= cameraToCenterLength;
// ----- Next, find the outline of the sphere, which is visible from the camera.
// We need to find cameraToPoint (the line from the camera to a point on the outline).
// cameraToPoint (= adjacent), radius (= opposite) and cameraToCenter (= hypotenuse)
// form a right-angled triangle.
// α is the angle between cameraToPoint and cameraToCenter:
float α = (float)Math.Asin(radius / cameraToCenterLength);
// Determine the normal of the triangle.
Vector3F normal = Vector3F.Cross(right, cameraToCenter);
if (normal.IsNumericallyZero)
{
normal = Vector3F.Up;
}
// adjacent = √(hypotenuse² + opposite²)
float radiusSquared = radius * radius;
float cameraToCenterLengthSquared = cameraToCenterLength * cameraToCenterLength;
float cameraToPointLength = (float)Math.Sqrt(cameraToCenterLengthSquared - radiusSquared);
Vector3F cameraToPoint = Matrix33F.CreateRotation(normal, α) * cameraToCenter;
// Get the first point on the outline in world space.
Vector3F pStart = cameraPose.Position + cameraToPoint * cameraToPointLength;
// Incrementally rotate the right vector 360° and repeat to find the remaining
// points on the outline.
const int NumberOfSegments = 32;
for (int i = 1; i <= NumberOfSegments; i++)
{
float β = i * ConstantsF.TwoPi / NumberOfSegments;
Vector3F rightRotated = Matrix33F.CreateRotation(forward, -β) * right;
normal = Vector3F.Cross(rightRotated, cameraToCenter);
if (normal.IsNumericallyZero)
{
normal = Vector3F.Up;
}
cameraToPointLength = (float)Math.Sqrt(cameraToCenterLengthSquared - radiusSquared);
cameraToPoint = Matrix33F.CreateRotation(normal, α) * cameraToCenter;
Vector3F pEnd = cameraPose.Position + cameraToPoint * cameraToPointLength;
_lineBatch.Add(pStart, pEnd, color);
pStart = pEnd;
}
}
// See FAST paper: "Interactive Continuous Collision Detection for Non-Convex Polyhedra", Kim Young et al.
/// <summary>
/// Gets the time of impact using Conservative Advancement.
/// </summary>
/// <param name="objectA">The object A.</param>
/// <param name="targetPoseA">The target pose of A.</param>
/// <param name="objectB">The object B.</param>
/// <param name="targetPoseB">The target pose of B.</param>
/// <param name="allowedPenetrationDepth">The allowed penetration depth.</param>
/// <param name="collisionDetection">The collision detection.</param>
/// <returns>
/// The time of impact in the range [0, 1].
/// </returns>
/// <remarks>
/// This algorithm does not work for concave objects.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="objectA"/>, <paramref name="objectB"/> or
/// <paramref name="collisionDetection"/> is <see langword="null"/>.
/// </exception>
internal static float GetTimeOfImpactCA(CollisionObject objectA, Pose targetPoseA,
CollisionObject objectB, Pose targetPoseB, float allowedPenetrationDepth,
CollisionDetection collisionDetection) // Required for collision algorithm matrix.
{
if (objectA == null)
{
throw new ArgumentNullException("objectA");
}
if (objectB == null)
{
throw new ArgumentNullException("objectB");
}
if (collisionDetection == null)
{
throw new ArgumentNullException("collisionDetection");
}
IGeometricObject geometricObjectA = objectA.GeometricObject;
IGeometricObject geometricObjectB = objectB.GeometricObject;
Pose startPoseA = geometricObjectA.Pose;
Pose startPoseB = geometricObjectB.Pose;
// Get angular velocity ω of object A (as magnitude + rotation axis).
// qEnd = ∆q * qStart
// => ∆q = qEnd * qStart.Inverse
QuaternionF qA = QuaternionF.CreateRotation(targetPoseA.Orientation * startPoseA.Orientation.Transposed);
// ω = ∆α / ∆t, ∆t = 1
// => ω = ∆α
float ωA = qA.Angle; // Magnitude |ω|
Vector3F ωAxisA = (!Numeric.AreEqual(qA.W, 1)) ? qA.Axis : Vector3F.UnitX; // Rotation axis of ω
// Get angular velocity ω of object B (as magnitude + rotation axis).
// (Same as above.)
QuaternionF qB = QuaternionF.CreateRotation(targetPoseB.Orientation * startPoseB.Orientation.Transposed);
float ωB = qB.Angle; // Magnitude |ω|
Vector3F ωAxisB = (!Numeric.AreEqual(qB.W, 1)) ? qB.Axis : Vector3F.UnitX; // Rotation axis of ω
// Bounding sphere radii.
float rMaxA = GetBoundingRadius(geometricObjectA);
float rMaxB = GetBoundingRadius(geometricObjectB);
// |ω| * rMax is the angular part of the projected velocity bound.
float angularVelocityProjected = ωA * rMaxA + ωB * rMaxB;
// Compute relative linear velocity.
// (linearRelVel ∙ normal > 0 if objects are getting closer.)
Vector3F linearVelocityA = targetPoseA.Position - startPoseA.Position;
Vector3F linearVelocityB = targetPoseB.Position - startPoseB.Position;
Vector3F linearVelocityRelative = linearVelocityA - linearVelocityB;
// Abort if relative movement is zero.
if (Numeric.IsZero(linearVelocityRelative.Length + angularVelocityProjected))
{
return(1);
}
var distanceAlgorithm = collisionDetection.AlgorithmMatrix[objectA, objectB];
// Use temporary test objects.
var testGeometricObjectA = TestGeometricObject.Create();
testGeometricObjectA.Shape = geometricObjectA.Shape;
testGeometricObjectA.Scale = geometricObjectA.Scale;
testGeometricObjectA.Pose = startPoseA;
var testGeometricObjectB = TestGeometricObject.Create();
testGeometricObjectB.Shape = geometricObjectB.Shape;
testGeometricObjectB.Scale = geometricObjectB.Scale;
testGeometricObjectB.Pose = startPoseB;
var testCollisionObjectA = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObjectA.SetInternal(objectA, testGeometricObjectA);
var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObjectB.SetInternal(objectB, testGeometricObjectB);
var testContactSet = ContactSet.Create(testCollisionObjectA, testCollisionObjectB);
try
{
distanceAlgorithm.UpdateClosestPoints(testContactSet, 0);
if (testContactSet.Count < 0)
{
// No distance result --> Abort.
return(1);
}
Vector3F normal = testContactSet[0].Normal;
float distance = -testContactSet[0].PenetrationDepth;
float λ = 0;
float λPrevious = 0;
for (int i = 0; i < MaxNumberOfIterations && distance > 0; i++)
{
// |v∙n|
float linearVelocityProject = Vector3F.Dot(linearVelocityRelative, normal);
// |n x ω| * rMax
angularVelocityProjected = Vector3F.Cross(normal, ωAxisA).Length *ωA *rMaxA
+ Vector3F.Cross(normal, ωAxisB).Length *ωB *rMaxB;
// Total projected velocity.
float velocityProjected = linearVelocityProject + angularVelocityProjected;
// Abort for separating objects.
if (Numeric.IsLess(velocityProjected, 0))
{
break;
}
// Increase TOI.
float μ = (distance + allowedPenetrationDepth) / velocityProjected;
λ = λ + μ;
if (λ < 0 || λ > 1)
{
break;
}
Debug.Assert(λPrevious < λ);
if (λ <= λPrevious)
{
break;
}
// Get new interpolated poses.
Vector3F positionA = startPoseA.Position + λ * (targetPoseA.Position - startPoseA.Position);
Matrix33F rotationA = Matrix33F.CreateRotation(ωAxisA, λ * ωA);
testGeometricObjectA.Pose = new Pose(positionA, rotationA * startPoseA.Orientation);
Vector3F positionB = startPoseB.Position + λ * (targetPoseB.Position - startPoseB.Position);
Matrix33F rotationB = Matrix33F.CreateRotation(ωAxisB, λ * ωB);
testGeometricObjectB.Pose = new Pose(positionB, rotationB * startPoseB.Orientation);
// Get new closest point distance.
distanceAlgorithm.UpdateClosestPoints(testContactSet, 0);
if (testContactSet.Count == 0)
{
break;
}
normal = testContactSet[0].Normal;
distance = -testContactSet[0].PenetrationDepth;
λPrevious = λ;
}
if (testContactSet.HaveContact && λ > 0 && λ < 1 && testContactSet.Count > 0)
{
return(λ);
// We already have a contact that we could use.
// result.Contact = testContactSet[0];
}
}
finally
{
// Recycle temporary objects.
testContactSet.Recycle(true);
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA);
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
testGeometricObjectA.Recycle();
testGeometricObjectB.Recycle();
}
return(1);
}
public override void Update(GameTime gameTime)
{
_domain.EnableMultithreading = InputService.IsDown(Keys.Space);
#if PROFILE
MessageBox.Show("Start");
_deltaTime = 1 / 60f;
for (int bla = 0; bla < 10000; bla++)
{
#endif
if (ClosestPointQueriesEnabled)
{
// Here we run closest point queries on all object pairs.
// We compare the results with the contact queries.
for (int i = 0; i < _domain.CollisionObjects.Count; i++)
{
for (int j = i + 1; j < _domain.CollisionObjects.Count; j++)
{
CollisionObject a = _domain.CollisionObjects[i];
CollisionObject b = _domain.CollisionObjects[j];
ContactSet closestPointQueryResult = _domain.CollisionDetection.GetClosestPoints(a, b);
ContactSet contactSet = _domain.GetContacts(a, b);
// Ignore height fields and rays.
if (a.GeometricObject.Shape is HeightField || b.GeometricObject.Shape is HeightField)
{
break;
}
if (a.GeometricObject.Shape is RayShape || b.GeometricObject.Shape is RayShape)
{
break;
}
if (contactSet == null || !contactSet.HaveContact)
{
// No contact in contactSet
if (closestPointQueryResult.HaveContact)
{
// Contact in closest point query. Results are inconsistent.
if (closestPointQueryResult.Count > 0 &&
closestPointQueryResult[0].PenetrationDepth > 0.001f)
{
Debugger.Break();
}
}
}
else if (!closestPointQueryResult.HaveContact)
{
// contact in contact query, but no contact in closest point query.
// We allow a deviation within a small tolerance.
if (closestPointQueryResult.Count > 0 && contactSet.Count > 0 &&
closestPointQueryResult[0].PenetrationDepth + contactSet[0].PenetrationDepth > 0.001f)
{
Debugger.Break();
}
}
}
}
}
// Reflect velocity if objects collide:
// The collision domain contains a ContactSet for each pair of touching objects.
foreach (var contactSet in _domain.ContactSets)
{
// Get the touching objects.
var moA = (MovingGeometricObject)contactSet.ObjectA.GeometricObject;
var moB = (MovingGeometricObject)contactSet.ObjectB.GeometricObject;
// Reflect only at boundary objects.
if (!(moA.Shape is PlaneShape) && !(moB.Shape is PlaneShape) &&
!(moA.Shape is HeightField) && !(moB.Shape is HeightField))
{
continue;
}
// Get normal vector. If objects are sensors, the contact set does not tell us
// the right normal.
Vector3F normal = Vector3F.Zero;
if (contactSet.Count > 0)
{
// Take normal from contact set.
normal = contactSet[0].Normal;
}
else
{
// If we use Trigger CollisionObjects we do not have contacts. --> Reflect at
// bounding planes.
if (moA.Shape is PlaneShape)
{
normal = ((PlaneShape)moA.Shape).Normal;
}
else if (moB.Shape is PlaneShape)
{
normal = -((PlaneShape)moB.Shape).Normal;
}
else if (moA.Shape is HeightField)
{
normal = Vector3F.UnitY;
}
else
{
normal = -Vector3F.UnitY;
}
}
//else if (moA.Shape is Plane || moB.Shape is Plane )
//{
// // Use plane normal.
// IGeometricObject plane = moA.Shape is Plane ? moA : moB;
// normal = plane.Pose.ToWorldDirection(((Plane)plane.Shape).Normal);
// if (moB == plane)
// normal = -normal;
//}
//else if (moA.Shape is HeightField || moB.Shape is HeightField)
//{
// // Use up-vector for height field contacts.
// normal = Vector3F.UnitY;
// if (moB.Shape is HeightField)
// normal = -normal;
//}
//else
//{
// // Use random normal.
// normal = RandomHelper.NextVector3F(-1, 1).Normalized;
//}
// Check if the objects move towards or away from each other in the direction of the normal.
if (normal != Vector3F.Zero && Vector3F.Dot(moB.LinearVelocity - moA.LinearVelocity, normal) <= 0)
{
// Objects move towards each other. --> Reflect their velocities.
moA.LinearVelocity -= 2 * Vector3F.ProjectTo(moA.LinearVelocity, normal);
moB.LinearVelocity -= 2 * Vector3F.ProjectTo(moB.LinearVelocity, normal);
moA.AngularVelocity = -moA.AngularVelocity;
moB.AngularVelocity = -moB.AngularVelocity;
}
}
// Get the size of the current time step.
float timeStep = (float)gameTime.ElapsedGameTime.TotalSeconds;
// Move objects.
var objects = _domain.CollisionObjects.Select(co => co.GeometricObject).OfType <MovingGeometricObject>();
foreach (var obj in objects)
{
// Update position.
Vector3F position = obj.Pose.Position + obj.LinearVelocity * timeStep;
// Update rotation.
Vector3F rotationAxis = obj.AngularVelocity;
float angularSpeed = obj.AngularVelocity.Length;
Matrix33F rotation = (Numeric.IsZero(angularSpeed))
? Matrix33F.Identity
: Matrix33F.CreateRotation(rotationAxis, angularSpeed * timeStep);
var orientation = rotation * obj.Pose.Orientation;
// Incrementally updating the rotation matrix will eventually create a
// matrix which is not a rotation matrix anymore because of numerical
// problems. Re-othogonalization make sure that the matrix represents a
// rotation.
orientation.Orthogonalize();
obj.Pose = new Pose(position, orientation);
}
// Update collision domain. This computes new contact information.
_domain.Update(timeStep);
#if PROFILE
MessageBox.Show("Finished");
Exit();
#endif
// Record some statistics.
int numberOfObjects = _domain.CollisionObjects.Count;
Profiler.SetFormat("NumObjects", 1, "The total number of objects.");
Profiler.AddValue("NumObjects", numberOfObjects);
// If there are n objects, we can have max. n * (n - 1) / 2 collisions.
Profiler.SetFormat("NumObjectPairs", 1, "The number of objects pairs, which have to be checked.");
Profiler.AddValue("NumObjectPairs", numberOfObjects * (numberOfObjects - 1f) / 2f);
// The first part of the collision detection is the "broad-phase" which
// filters out objects that cannot collide (e.g. using a fast bounding box test).
Profiler.SetFormat("BroadPhasePairs", 1, "The number of overlaps reported by the broad phase.");
Profiler.AddValue("BroadPhasePairs", _domain.NumberOfBroadPhaseOverlaps);
// Finally, the collision detection computes the exact contact information and creates
// a ContactSet with the Contacts for each pair of colliding objects.
Profiler.SetFormat("ContactSetCount", 1, "The number of actual collisions.");
Profiler.AddValue("ContactSetCount", _domain.ContactSets.Count);
// Draw objects using the DebugRenderer of the graphics screen.
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
foreach (var collisionObject in _domain.CollisionObjects)
{
debugRenderer.DrawObject(collisionObject.GeometricObject, GraphicsHelper.GetUniqueColor(collisionObject), false, false);
}
}
}
/// <summary>
/// Performs more collision tests while slightly rotating one collision object.
/// </summary>
/// <param name="collisionDetection">The collision detection.</param>
/// <param name="contactSet">
/// The contact set; must contain at least 1 <see cref="Contact"/>.
/// </param>
/// <param name="perturbB">
/// if set to <see langword="true"/> collision object B will be rotated; otherwise collision
/// object A will be rotated.
/// </param>
/// <param name="testMethod">The test method that is called to compute contacts.</param>
/// <remarks>
/// This method rotates one object 3 times and calls contact computation for the new
/// orientations. It is recommended to call this method only when the contact set has 1 new
/// contact.
/// </remarks>
internal static void TestWithPerturbations(CollisionDetection collisionDetection, ContactSet contactSet, bool perturbB, Action <ContactSet> testMethod)
{
Debug.Assert(contactSet != null);
Debug.Assert(contactSet.Count > 0 && contactSet.HaveContact || !contactSet.IsPerturbationTestAllowed);
Debug.Assert(testMethod != null);
// Make this test only if there is 1 contact.
// If there are 0 contacts, we assume that the contact pair is separated.
// If there are more than 3 contacts, then we already have a lot of contacts to work with, no
// need to search for more.
if (!contactSet.HaveContact || contactSet.Count == 0 || contactSet.Count >= 4 || !contactSet.IsPerturbationTestAllowed)
{
return;
}
// Get data of object that will be rotated.
var collisionObject = (perturbB) ? contactSet.ObjectB : contactSet.ObjectA;
var geometricObject = collisionObject.GeometricObject;
var pose = geometricObject.Pose;
// Get normal, pointing to the test object.
var normal = contactSet[0].Normal;
if (!perturbB)
{
normal = -normal;
}
var contactPosition = contactSet[0].Position;
var centerToContact = contactPosition - pose.Position;
// Compute a perturbation angle proportional to the dimension of the object.
var radius = geometricObject.Aabb.Extent.Length;
var angle = collisionDetection.ContactPositionTolerance / radius;
// axis1 is in the contact tangent plane, orthogonal to normal.
var axis1 = Vector3F.Cross(normal, centerToContact);
// If axis1 is zero then normal and centerToContact are collinear. This happens
// for example for spheres or cone tips against flat faces. In these cases we assume
// that there will be max. 1 contact.
if (axis1.IsNumericallyZero)
{
return;
}
var axis1Local = pose.ToLocalDirection(axis1);
var rotation = Matrix33F.CreateRotation(axis1Local, -angle);
// Use temporary test objects.
var testGeometricObject = TestGeometricObject.Create();
testGeometricObject.Shape = geometricObject.Shape;
testGeometricObject.Scale = geometricObject.Scale;
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation);
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(collisionObject, testGeometricObject);
var testContactSet = perturbB ? ContactSet.Create(contactSet.ObjectA, testCollisionObject)
: ContactSet.Create(testCollisionObject, contactSet.ObjectB);
testContactSet.IsPerturbationTestAllowed = false; // Avoid recursive perturbation tests!
testContactSet.PreferredNormal = contactSet.PreferredNormal;
// Compute next contacts.
testMethod(testContactSet);
if (testContactSet.Count > 0)
{
// axis2 is in the contact tangent plane, orthogonal to axis1.
var axis2 = Vector3F.Cross(axis1, normal);
var axis2Local = pose.ToLocalDirection(axis2);
var rotation2 = Matrix33F.CreateRotation(axis2Local, -angle);
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2);
// Compute next contacts.
testMethod(testContactSet);
// Invert rotation2.
rotation2.Transpose();
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2);
// Compute next contacts.
testMethod(testContactSet);
}
// Set HaveContact. It is reset when a perturbation separates the objects.
testContactSet.HaveContact = true;
// TODO: Test if we need this:
// The contact world positions are not really correct because one object was rotated.
// UpdateContacts recomputes the world positions from the local positions.
UpdateContacts(testContactSet, 0, collisionDetection.ContactPositionTolerance);
// Merge contacts of testContactSet into contact set, but do not change existing contacts.
foreach (var contact in testContactSet)
{
// We call TryMerge() to see if the contact is similar to an existing contact.
bool exists = TryMergeWithNearestContact(
contactSet,
contact,
collisionDetection.ContactPositionTolerance,
false); // The existing contact must no be changed!
if (exists)
{
// We can throw away the new contact because a similar is already in the contact set.
contact.Recycle();
}
else
{
// Add new contact.
contactSet.Add(contact);
}
}
// Recycle temporary objects.
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
}
