/// <summary>
/// Initializes a new instance of the <see cref="Wheel"/> class.
/// </summary>
public Wheel()
{
Radius = 0.4f;
SuspensionRestLength = 0.6f;
MinSuspensionLength = float.NegativeInfinity;
SuspensionLength = SuspensionRestLength;
PreviousSuspensionLength = SuspensionRestLength;
SuspensionStiffness = 20;
SuspensionCompressionDamping = 4f;
SuspensionRelaxationDamping = 3f;
MaxSuspensionForce = 6000;
RollingFrictionForce = 500;
Friction = 0.9f;
RollReduction = 0.3f;
Vector3 rayOrigin = Vector3.Zero;
Vector3 rayDirection = -Vector3.UnitY;
float rayLength = Radius + SuspensionRestLength;
Ray = new RayShape(rayOrigin, rayDirection, rayLength)
{
StopsAtFirstHit = true,
};
GeometricObject = new GeometricObject(Ray);
CollisionObject = new CollisionObject(GeometricObject);
}
public override void Update(GameTime gameTime)
{
if (InputService.IsPressed(MouseButtons.Left, true) || InputService.IsPressed(Buttons.RightTrigger, true, LogicalPlayerIndex.One))
{
var cameraPose = GraphicsScreen.CameraNode.PoseWorld;
Vector3 cameraPosition = cameraPose.Position;
Vector3 cameraDirection = cameraPose.ToWorldDirection(Vector3.Forward);
// Create a ray for picking.
RayShape ray = new RayShape(cameraPosition, cameraDirection, 1000);
// The ray should stop at the first hit. We only want the first object.
ray.StopsAtFirstHit = true;
// The collision detection requires a CollisionObject.
CollisionObject rayCollisionObject = new CollisionObject(new GeometricObject(ray, Pose.Identity));
// Get the first object that has contact with the ray.
ContactSet contactSet = Simulation.CollisionDomain.GetContacts(rayCollisionObject).FirstOrDefault();
if (contactSet != null && contactSet.Count > 0)
{
// The ray has hit something.
// The contact set contains all detected contacts between the ray and the rigid body.
// Get the first contact in the contact set. (A ray hit usually contains exactly 1 contact.)
Contact contact = contactSet[0];
var hitPosition = contact.Position;
var normal = contact.Normal;
if (contactSet.ObjectA == rayCollisionObject)
{
normal = -normal;
}
// The particle parameter arrays are circular buffers. Get the particle array index
// where the next particle is created:
int particleIndex = (_decals.ParticleStartIndex + _decals.NumberOfActiveParticles) % _decals.MaxNumberOfParticles;
// Add 1 particle.
int numberOfCreatedParticles = _decals.AddParticles(1, null);
if (numberOfCreatedParticles > 0)
{
// We initialize the particle parameters Position, Normal and Axis manually using
// the results of the collision detection:
var positionParameter = _decals.Parameters.Get <Vector3>(ParticleParameterNames.Position);
positionParameter.Values[particleIndex] = hitPosition + normal * 0.01f; // We add a slight 1 cm offset to avoid z-fighting.
var normalParameter = _decals.Parameters.Get <Vector3>("Normal");
normalParameter.Values[particleIndex] = normal;
var axisParameter = _decals.Parameters.Get <Vector3>("Axis");
axisParameter.Values[particleIndex] = (normal == Vector3.Up) ? Vector3.Backward : Vector3.Up;
}
}
}
// Synchronize particles <-> graphics.
_particleSystemNode.Synchronize(GraphicsService);
Profiler.AddValue("ParticleCount", ParticleHelper.CountNumberOfParticles(ParticleSystemService.ParticleSystems));
}
public void SerializationXml()
{
var a = new RayShape(new Vector3F(1, 2, 3), new Vector3F(2, 3, 4).Normalized, 1234.567f);
a.StopsAtFirstHit = true;
// Serialize object.
var stream = new MemoryStream();
var serializer = new XmlSerializer(typeof(Shape));
serializer.Serialize(stream, a);
// Output generated xml. Can be manually checked in output window.
stream.Position = 0;
var xml = new StreamReader(stream).ReadToEnd();
Trace.WriteLine("Serialized Object:\n" + xml);
// Deserialize object.
stream.Position = 0;
var deserializer = new XmlSerializer(typeof(Shape));
var b = (RayShape)deserializer.Deserialize(stream);
Assert.AreEqual(a.Direction, b.Direction);
Assert.AreEqual(a.Length, b.Length);
Assert.AreEqual(a.Origin, b.Origin);
Assert.AreEqual(a.StopsAtFirstHit, b.StopsAtFirstHit);
}
public void TestProperties()
{
RayShape l = new RayShape();
Assert.AreEqual(new Vector3F(), l.Origin);
Assert.AreEqual(new Vector3F(1, 0, 0), l.Direction);
l.Origin = new Vector3F(1, 2, 3);
Assert.AreEqual(new Vector3F(1, 2, 3), l.Origin);
Assert.AreEqual(new Vector3F(1, 0, 0), l.Direction);
l.Direction = new Vector3F(4, 5, 6).Normalized;
Assert.AreEqual(new Vector3F(1, 2, 3), l.Origin);
Assert.AreEqual(new Vector3F(4, 5, 6).Normalized, l.Direction);
l.Length = 11;
Assert.AreEqual(new Vector3F(1, 2, 3), l.Origin);
Assert.AreEqual(new Vector3F(4, 5, 6).Normalized, l.Direction);
Assert.AreEqual(11, l.Length);
Assert.AreEqual(false, l.StopsAtFirstHit);
l.StopsAtFirstHit = true;
Assert.AreEqual(new Vector3F(1, 2, 3), l.Origin);
Assert.AreEqual(new Vector3F(4, 5, 6).Normalized, l.Direction);
Assert.AreEqual(11, l.Length);
Assert.AreEqual(true, l.StopsAtFirstHit);
}
/// <summary>
/// Initializes a new instance of the <see cref="ConstraintWheel"/> class.
/// </summary>
public ConstraintWheel()
{
_radius = 0.4f;
_suspensionRestLength = 0.6f;
MinSuspensionLength = float.NegativeInfinity;
SuspensionLength = SuspensionRestLength;
SuspensionStiffness = 100;
SuspensionDamping = 10;
MaxSuspensionForce = float.PositiveInfinity;
RollingFrictionForce = 500;
Friction = 1.1f;
RollReduction = 0.3f;
Vector3 rayOrigin = Vector3.Zero;
Vector3 rayDirection = -Vector3.UnitY;
float rayLength = Radius + SuspensionRestLength;
_ray = new RayShape(rayOrigin, rayDirection, rayLength)
{
StopsAtFirstHit = true,
};
CollisionObject = new CollisionObject(this);
Constraint = new WheelConstraint(this);
}
public void SetData(RayShape shape)
{
currentShape = shape;
float hue;
Color.RGBToHSV(shape.color, out hue, out _, out _);
OnValueChanged(hue);
slider.value = hue;
}
public void SetValue(RayShape shape)
{
if (input == null)
{
Init();
}
currentShape = shape;
input.text = shape.Scale[Index].ToString("g4");
}
public void GetSupportPoint()
{
RayShape r = new RayShape(new Vector3F(1, 0, 0), new Vector3F(1, 1, 0).Normalized, 10);
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(1, 0, 0), r.GetSupportPointNormalized(new Vector3F(-1, 0, 0))));
Assert.IsTrue(Vector3F.AreNumericallyEqual(r.Origin + r.Direction * r.Length, r.GetSupportPointNormalized(new Vector3F(1, 0, 0))));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(1, 0, 0), r.GetSupportPoint(new Vector3F(-2, 0, 0))));
Assert.IsTrue(Vector3F.AreNumericallyEqual(r.Origin + r.Direction * r.Length, r.GetSupportPoint(new Vector3F(2, 0, 0))));
}
public void SetOptions(RayShape shape)
{
if (dropdown == null)
{
Init();
}
currentShape = shape;
dropdown.SetValueWithoutNotify((int)shape.shapeType);
}
public void GetMesh()
{
var r = new RayShape(new Vector3F(1, 2, 3), Vector3F.UnitY, 10);
var m = r.GetMesh(0, 1);
Assert.AreEqual(1, m.NumberOfTriangles);
Triangle t = m.GetTriangle(0);
Assert.IsTrue(r.Origin == t.Vertex0);
Assert.IsTrue(r.Origin + r.Direction * r.Length == t.Vertex2);
}
public void Clone()
{
RayShape ray = new RayShape(new Vector3F(1, 2, 3), new Vector3F(2, 3, 4).Normalized, 1234.567f);
RayShape clone = ray.Clone() as RayShape;
Assert.IsNotNull(clone);
Assert.AreEqual(ray.Origin, clone.Origin);
Assert.AreEqual(ray.Direction, clone.Direction);
Assert.AreEqual(ray.Length, clone.Length);
Assert.AreEqual(ray.GetAabb(Pose.Identity).Minimum, clone.GetAabb(Pose.Identity).Minimum);
Assert.AreEqual(ray.GetAabb(Pose.Identity).Maximum, clone.GetAabb(Pose.Identity).Maximum);
}
/// <summary>
/// Initializes a new instance of <see cref="Ray"/> from a <see cref="RayShape"/>.
/// </summary>
/// <param name="rayShape">
/// The <see cref="RayShape"/> from which origin and direction are copied.
/// </param>
/// <exception cref="ArgumentNullException">
/// <paramref name="rayShape"/> is <see langword="null"/>.
/// </exception>
public Ray(RayShape rayShape)
{
if (rayShape == null)
{
throw new ArgumentNullException("rayShape");
}
Origin = rayShape.Origin;
Direction = rayShape.Direction;
Length = rayShape.Length;
}
public void Clone()
{
RayShape ray = new RayShape(new Vector3F(1, 2, 3), new Vector3F(2, 3, 4).Normalized, 1234.567f);
RayShape clone = ray.Clone() as RayShape;
Assert.IsNotNull(clone);
Assert.AreEqual(ray.Origin, clone.Origin);
Assert.AreEqual(ray.Direction, clone.Direction);
Assert.AreEqual(ray.Length, clone.Length);
Assert.AreEqual(ray.GetAabb(Pose.Identity).Minimum, clone.GetAabb(Pose.Identity).Minimum);
Assert.AreEqual(ray.GetAabb(Pose.Identity).Maximum, clone.GetAabb(Pose.Identity).Maximum);
}
public void GetMesh()
{
var r = new RayShape(new Vector3F(1, 2, 3), Vector3F.UnitY, 10);
var m = r.GetMesh(0, 1);
Assert.AreEqual(1, m.NumberOfTriangles);
Triangle t = m.GetTriangle(0);
Assert.IsTrue(r.Origin == t.Vertex0);
Assert.IsTrue(r.Origin + r.Direction * r.Length == t.Vertex2);
}
// OnUpdate() is called once per frame.
protected override void OnUpdate(TimeSpan deltaTime)
{
if (_inputService.IsPressed(MouseButtons.Middle, true) || _inputService.IsPressed(Buttons.RightShoulder, true, LogicalPlayerIndex.One))
{
// The user has triggered an explosion.
// The explosion is created at the position that is targeted with the cross-hair.
// We can perform a ray hit-test to find the position. The ray starts at the camera
// position and shoots forward (-z direction).
var cameraGameObject = (CameraObject)_gameObjectService.Objects["Camera"];
var cameraNode = cameraGameObject.CameraNode;
Vector3 cameraPosition = cameraNode.PoseWorld.Position;
Vector3 cameraDirection = cameraNode.PoseWorld.ToWorldDirection(Vector3.Forward);
// Create a ray for hit-testing.
var ray = new RayShape(cameraPosition, cameraDirection, 1000);
// The ray should stop at the first hit. We only want the first object.
ray.StopsAtFirstHit = true;
// The collision detection requires a CollisionObject.
var rayCollisionObject = new CollisionObject(new GeometricObject(ray, Pose.Identity))
{
// In SampleGame.ResetPhysicsSimulation() a collision filter was set:
// CollisionGroup = 0 ... objects that support hit-testing
// CollisionGroup = 1 ... objects that are ignored during hit-testing
// CollisionGroup = 2 ... objects (rays) for hit-testing
CollisionGroup = 2,
};
// Get the first object that has contact with the ray.
ContactSet contactSet = _simulation.CollisionDomain.GetContacts(rayCollisionObject).FirstOrDefault();
if (contactSet != null && contactSet.Count > 0)
{
// The ray has hit something.
// The contact set contains all detected contacts between the ray and another object.
// Get the first contact in the contact set. (A ray hit usually contains exactly 1 contact.)
Contact contact = contactSet[0];
// Create an explosion at the hit position.
var explosion = new Explosion {
Position = contact.Position
};
_simulation.ForceEffects.Add(explosion);
// Note: The Explosion force effect removes itself automatically from the simulation once
// it has finished.
}
}
}
public void SetData(RayShape shape)
{
if (Panel == null)
{
Init();
}
currentShape = shape;
SizeInput[] inputs = Panel.GetComponentsInChildren <SizeInput>();
foreach (SizeInput input in inputs)
{
input.SetValue(currentShape);
}
}
public void SerializationBinary()
{
var a = new RayShape(new Vector3F(1, 2, 3), new Vector3F(2, 3, 4).Normalized, 1234.567f);
a.StopsAtFirstHit = true;
// Serialize object.
var stream = new MemoryStream();
var formatter = new BinaryFormatter();
formatter.Serialize(stream, a);
// Deserialize object.
stream.Position = 0;
var deserializer = new BinaryFormatter();
var b = (RayShape)deserializer.Deserialize(stream);
Assert.AreEqual(a.Direction, b.Direction);
Assert.AreEqual(a.Length, b.Length);
Assert.AreEqual(a.Origin, b.Origin);
Assert.AreEqual(a.StopsAtFirstHit, b.StopsAtFirstHit);
}
public void DirectionException()
{
RayShape l = new RayShape();
l.Direction = new Vector3F();
}
public void LengthException2()
{
RayShape l = new RayShape();
l.Length = float.NegativeInfinity;
}
public void SerializationBinary()
{
var a = new RayShape(new Vector3F(1, 2, 3), new Vector3F(2, 3, 4).Normalized, 1234.567f);
a.StopsAtFirstHit = true;
// Serialize object.
var stream = new MemoryStream();
var formatter = new BinaryFormatter();
formatter.Serialize(stream, a);
// Deserialize object.
stream.Position = 0;
var deserializer = new BinaryFormatter();
var b = (RayShape)deserializer.Deserialize(stream);
Assert.AreEqual(a.Direction, b.Direction);
Assert.AreEqual(a.Length, b.Length);
Assert.AreEqual(a.Origin, b.Origin);
Assert.AreEqual(a.StopsAtFirstHit, b.StopsAtFirstHit);
}
public void DirectionException()
{
RayShape l = new RayShape();
l.Direction = new Vector3F();
}
protected override void OnHandleInput(InputContext context)
{
if (_cameraObject.CameraNode == null)
{
return;
}
// The input context contains the mouse position that is used by the UI controls of this
// screen. The mouse position is stored in the following properties:
// - context.ScreenMousePosition
// - context.ScreenMousePositionDelta
// - context.MousePosition
// - context.MousePositionDelta
//
// Currently, these properties contain the mouse position relative to the game window.
// But the mouse position of the in-game screen is determined by the reticle of the
// game camera. We need to make a ray-cast to see which part of the screen is hit and
// override the properties.
bool screenHit = false;
// Get the camera position and the view direction in world space.
Vector3 cameraPosition = _cameraObject.CameraNode.PoseWorld.Position;
Vector3 cameraDirection = _cameraObject.CameraNode.PoseWorld.ToWorldDirection(Vector3.Forward);
// Create a ray (ideally this shape should be cached and reused).
var ray = new RayShape(cameraPosition, cameraDirection, 1000);
// We are only interested in the first object that is hit by the ray.
ray.StopsAtFirstHit = true;
// Create a collision object for this shape.
var rayCollisionObject = new CollisionObject(new GeometricObject(ray, Pose.Identity));
// Use the CollisionDomain of the physics simulation to perform a ray cast.
ContactSet contactSet = _simulation.CollisionDomain.GetContacts(rayCollisionObject).FirstOrDefault();
if (contactSet != null && contactSet.Count > 0)
{
// We have hit something :-)
// Get the contact information of the ray hit.
Contact contact = contactSet[0];
// Get the hit object (one object in the contact set is the ray and the other object is the hit object).
CollisionObject hitCollisionObject = (contactSet.ObjectA == rayCollisionObject) ? contactSet.ObjectB : contactSet.ObjectA;
RigidBody hitBody = hitCollisionObject.GeometricObject as RigidBody;
if (hitBody != null && hitBody.UserData is string && (string)hitBody.UserData == "TV")
{
// We have hit a dynamic rigid body of a TV object.
// Get the normal vector of the contact.
var normal = (contactSet.ObjectA == rayCollisionObject) ? -contact.Normal : contact.Normal;
// Convert the normal vector to the local space of the TV box.
normal = hitBody.Pose.ToLocalDirection(normal);
// The InGameUIScreen texture is only mapped onto the -Y sides of the boxes. If the user
// looks onto another side, he cannot interact with the game screen.
if (normal.Y < 0.5f)
{
// The user looks onto the TV's front side. Now, we have to map the ray hit position
// to the texture coordinate of the InGameUIScreen render target/texture.
var localHitPosition = (contactSet.ObjectA == rayCollisionObject) ? contact.PositionBLocal : contact.PositionALocal;
var normalizedPosition = GetTextureCoordinate(localHitPosition);
// The texture coordinate is in the range [0, 0] to [1, 1]. If we multiply it with the
// screen extent to the position in pixels.
var inGameScreenMousePosition = normalizedPosition * new Vector2F(ActualWidth, ActualHeight);
var inGameScreenMousePositionDelta = inGameScreenMousePosition - _lastMousePosition;
// Finally, we can set the mouse positions that are relative to the InGame screen. Hurray!
context.ScreenMousePosition = inGameScreenMousePosition;
context.ScreenMousePositionDelta = inGameScreenMousePositionDelta;
context.MousePosition = inGameScreenMousePosition;
context.MousePositionDelta = inGameScreenMousePositionDelta;
// Store the mouse position so that we can compute MousePositionDelta in the next frame.
_lastMousePosition = context.MousePosition;
screenHit = true;
}
}
}
if (screenHit)
{
// Call base class to call HandleInput for all child controls. The child controls will
// use the overridden mouse positions.
base.OnHandleInput(context);
}
}
public void LengthException2()
{
RayShape l = new RayShape();
l.Length = float.NegativeInfinity;
}
public void LengthException()
{
RayShape l = new RayShape();
l.Length = float.PositiveInfinity;
}
public void LengthException()
{
RayShape l = new RayShape();
l.Length = float.PositiveInfinity;
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
if (type == CollisionQueryType.ClosestPoints)
{
// Just use normal height field shape algorithm.
_heightFieldAlgorithm.ComputeCollision(contactSet, type);
return;
}
Debug.Assert(type != CollisionQueryType.ClosestPoints, "Closest point queries should have already been handled!");
// HeightField = A, Ray = B
IGeometricObject heightFieldObject = contactSet.ObjectA.GeometricObject;
IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;
// Object A should be the height field, swap objects if necessary.
bool swapped = (heightFieldObject.Shape is RayShape);
if (swapped)
{
MathHelper.Swap(ref rayObject, ref heightFieldObject);
}
RayShape rayShape = rayObject.Shape as RayShape;
HeightField heightField = heightFieldObject.Shape as HeightField;
// Check if shapes are correct.
if (rayShape == null || heightField == null)
{
throw new ArgumentException("The contact set must contain a ray and a height field.", "contactSet");
}
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3F rayScale = rayObject.Scale;
Pose rayPose = rayObject.Pose;
Vector3F heightFieldScale = heightFieldObject.Scale;
Pose heightFieldPose = heightFieldObject.Pose;
// We do not support negative scaling. It is not clear what should happen when y is
// scaled with a negative factor and triangle orders would be wrong... Not worth the trouble.
if (heightFieldScale.X < 0 || heightFieldScale.Y < 0 || heightFieldScale.Z < 0)
{
throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported.");
}
// Ray in world space.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale);
rayWorld.ToWorld(ref rayPose);
// Ray in local scaled space of the height field.
Ray rayScaled = rayWorld;
rayScaled.ToLocal(ref heightFieldPose);
// Ray in local unscaled space of the mesh.
Ray rayUnscaled = rayScaled;
var inverseCompositeScale = Vector3F.One / heightFieldScale;
rayUnscaled.Scale(ref inverseCompositeScale);
// Get height field and basic info.
int arrayLengthX = heightField.NumberOfSamplesX;
int arrayLengthZ = heightField.NumberOfSamplesZ;
int numberOfCellsX = arrayLengthX - 1;
int numberOfCellsZ = arrayLengthZ - 1;
Debug.Assert(arrayLengthX > 1 && arrayLengthZ > 1, "A height field should contain at least 2 x 2 elements (= 1 cell).");
float cellWidthX = heightField.WidthX / numberOfCellsX; // Unscaled!
float cellWidthZ = heightField.WidthZ / numberOfCellsZ; // Unscaled!
// We use a 2D-DDA traversal of the height field cells. In other words: Look at it from
// above. The height field is our screen and we will select the cells as if we draw
// a pixel line. This could be made more efficient when we do not recompute values and
// reuse values and make incremental steps Bresenham-style.
// See GeometryHelper_Casts.cs method HaveContact(Aabb, ray) for explanation of the
// ray parameter formula.
var rayUnscaledDirectionInverse = new Vector3F(
1 / rayUnscaled.Direction.X,
1 / rayUnscaled.Direction.Y,
1 / rayUnscaled.Direction.Z);
// The position where the ray enters the current cell.
var cellEnter = rayUnscaled.Origin; // Unscaled!!!
var originX = heightField.OriginX;
var originZ = heightField.OriginZ;
// ----- Find first cell.
int indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1; // (int)(...) does not return the desired result for negative values!
if (indexX < 0)
{
if (rayUnscaled.Direction.X <= 0)
{
return;
}
float parameter = (originX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
if (parameter > rayUnscaled.Length)
{
return; // The ray does not reach the height field.
}
cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
indexX = 0;
}
else if (indexX >= numberOfCellsX)
{
if (rayUnscaled.Direction.X >= 0)
{
return;
}
float parameter = (originX + heightField.WidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
if (parameter > rayUnscaled.Length)
{
return; // The ray does not reach the height field.
}
cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
indexX = numberOfCellsX - 1;
}
int indexZ = (cellEnter.Z >= originZ) ? (int)((cellEnter.Z - originZ) / cellWidthZ) : -1;
if (indexZ < 0)
{
if (rayUnscaled.Direction.Z <= 0)
{
return;
}
float parameter = (originZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
if (parameter > rayUnscaled.Length)
{
return; // The ray does not reach the next height field.
}
cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
// We also have to correct the indexX!
indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1;
indexZ = 0;
}
else if (indexZ >= numberOfCellsZ)
{
if (rayUnscaled.Direction.Z >= 0)
{
return;
}
float parameter = (originZ + heightField.WidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
if (parameter > rayUnscaled.Length)
{
return; // The ray does not reach the next height field.
}
cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1;
indexZ = numberOfCellsZ - 1;
}
if (indexX < 0 || indexX >= numberOfCellsX || indexZ < 0 || indexZ >= numberOfCellsZ)
{
return;
}
while (true)
{
// ----- Get triangles of current cell.
var triangle0 = heightField.GetTriangle(indexX, indexZ, false);
var triangle1 = heightField.GetTriangle(indexX, indexZ, true);
// Index of first triangle.
var triangleIndex = (indexZ * numberOfCellsX + indexX) * 2;
float xRelative = (cellEnter.X - originX) / cellWidthX - indexX;
float zRelative = (cellEnter.Z - originZ) / cellWidthZ - indexZ;
bool enterSecondTriangle = (xRelative + zRelative) > 1; // The diagonal is where xRel + zRel == 1.
// ----- Find cell exit and move indices to next cell.
// The position where the ray leaves the current cell.
Vector3F cellExit;
float nextXParameter = float.PositiveInfinity;
if (rayUnscaled.Direction.X > 0)
{
nextXParameter = (originX + (indexX + 1) * cellWidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
}
else if (rayUnscaled.Direction.X < 0)
{
nextXParameter = (originX + indexX * cellWidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
}
float nextZParameter = float.PositiveInfinity;
if (rayUnscaled.Direction.Z > 0)
{
nextZParameter = (originZ + (indexZ + 1) * cellWidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
}
else if (rayUnscaled.Direction.Z < 0)
{
nextZParameter = (originZ + indexZ * cellWidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
}
bool isLastCell = false;
if (nextXParameter < nextZParameter)
{
if (rayUnscaled.Direction.X > 0)
{
indexX++;
if (indexX >= numberOfCellsX) // Abort if we have left the height field.
{
isLastCell = true;
}
}
else
{
indexX--;
if (indexX < 0)
{
isLastCell = true;
}
}
if (nextXParameter > rayUnscaled.Length)
{
isLastCell = true; // The ray does not reach the next cell.
nextXParameter = rayUnscaled.Length;
}
cellExit = rayUnscaled.Origin + nextXParameter * rayUnscaled.Direction;
}
else
{
if (rayUnscaled.Direction.Z > 0)
{
indexZ++;
if (indexZ >= numberOfCellsZ)
{
isLastCell = true;
}
}
else
{
indexZ--;
if (indexZ < 0)
{
isLastCell = true;
}
}
if (nextZParameter > rayUnscaled.Length)
{
isLastCell = true;
nextZParameter = rayUnscaled.Length;
}
cellExit = rayUnscaled.Origin + nextZParameter * rayUnscaled.Direction;
}
// ----- We can skip cell if cell AABB is below the ray.
var rayMinY = Math.Min(cellEnter.Y, cellExit.Y) - CollisionDetection.Epsilon; // Apply to avoid missing collisions when ray hits a cell border.
// The ray is above if no height field height is higher the ray height.
// (This check handles NaN height values (holes) correctly.)
bool rayIsAbove = !(triangle0.Vertex0.Y >= rayMinY ||
triangle0.Vertex1.Y >= rayMinY ||
triangle0.Vertex2.Y >= rayMinY ||
triangle1.Vertex1.Y >= rayMinY); // Vertex1 of triangle1 is the fourth quad vertex!
// ----- Test ray against the 2 triangles of the cell.
bool triangle0IsHole = false;
bool triangle1IsHole = false;
if (!rayIsAbove)
{
// Abort if a height value is NaN (hole).
triangle0IsHole = Numeric.IsNaN(triangle0.Vertex0.Y * triangle0.Vertex1.Y * triangle0.Vertex2.Y);
triangle1IsHole = Numeric.IsNaN(triangle1.Vertex0.Y * triangle1.Vertex1.Y * triangle1.Vertex2.Y);
bool contactAdded = false;
if (enterSecondTriangle)
{
// Test second triangle first.
if (!triangle1IsHole)
{
contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle1, triangleIndex + 1, ref heightFieldPose, ref heightFieldScale);
}
if (!contactAdded && !triangle0IsHole)
{
contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle0, triangleIndex, ref heightFieldPose, ref heightFieldScale);
}
}
else
{
// Test first triangle first.
if (!triangle0IsHole)
{
contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle0, triangleIndex, ref heightFieldPose, ref heightFieldScale);
}
if (!contactAdded && !triangle1IsHole)
{
contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle1, triangleIndex + 1, ref heightFieldPose, ref heightFieldScale);
}
}
if (contactAdded)
{
return;
}
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
{
return;
}
}
// ----- Return simplified contact if cellEnter is below the cell.
if (!rayIsAbove)
{
if (!enterSecondTriangle && !triangle0IsHole && GeometryHelper.IsInFront(triangle0, cellEnter) < 0 ||
enterSecondTriangle && !triangle1IsHole && GeometryHelper.IsInFront(triangle1, cellEnter) < 0)
{
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
{
return;
}
var position = heightFieldPose.ToWorldPosition(cellEnter * heightFieldScale);
var normal = heightFieldPose.ToWorldDirection(Vector3F.UnitY);
if (swapped)
{
normal = -normal;
}
float penetrationDepth = (position - rayWorld.Origin).Length;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, true);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
return;
}
}
// ----- Move to next cell.
if (isLastCell)
{
return;
}
cellEnter = cellExit;
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
Debug.Assert(contactSet.Count <= 1, "Ray vs. plane should have at max 1 contact.");
// Object A should be the plane.
// Object B should be the ray.
IGeometricObject planeObject = contactSet.ObjectA.GeometricObject;
IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;
// Swap objects if necessary.
bool swapped = (rayObject.Shape is PlaneShape);
if (swapped)
{
MathHelper.Swap(ref planeObject, ref rayObject);
}
PlaneShape planeShape = planeObject.Shape as PlaneShape;
RayShape rayShape = rayObject.Shape as RayShape;
// Check if A is really a plane and B is a ray.
if (planeShape == null || rayShape == null)
{
throw new ArgumentException("The contact set must contain a plane and a ray.", "contactSet");
}
// Get transformations.
Vector3F planeScale = planeObject.Scale;
Vector3F rayScale = rayObject.Scale;
Pose rayPose = rayObject.Pose;
Pose planePose = planeObject.Pose;
// Apply scale to plane.
Plane plane = new Plane(planeShape);
plane.Scale(ref planeScale);
// Apply scale to ray and transform ray into local space of plane.
Ray ray = new Ray(rayShape);
ray.Scale(ref rayScale); // Scale ray.
ray.ToWorld(ref rayPose); // Transform ray to world space.
ray.ToLocal(ref planePose); // Transform ray to local space of plane.
// Convert ray into a line segment.
LineSegment segment = new LineSegment {
Start = ray.Origin, End = ray.Origin + ray.Direction * ray.Length
};
// Check if ray origin is inside the plane. Otherwise call plane vs. ray query.
Vector3F linePoint;
Vector3F planePoint = Vector3F.Zero;
if (Vector3F.Dot(segment.Start, plane.Normal) <= plane.DistanceFromOrigin)
{
// The origin of the ray is below the plane.
linePoint = segment.Start;
contactSet.HaveContact = true;
}
else
{
// The origin of the ray is above the plane.
contactSet.HaveContact = GeometryHelper.GetClosestPoints(plane, segment, out linePoint, out planePoint);
}
if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
{
// HaveContact queries can exit here.
// GetContacts queries can exit here if we don't have a contact.
return;
}
// ----- Create contact info.
Vector3F position;
float penetrationDepth;
if (contactSet.HaveContact)
{
// We have a contact.
position = planePose.ToWorldPosition(linePoint);
penetrationDepth = (linePoint - segment.Start).Length;
}
else
{
// Closest points, but separated.
position = planePose.ToWorldPosition((planePoint + linePoint) / 2);
penetrationDepth = -(linePoint - planePoint).Length;
}
Vector3F normal = planePose.ToWorldDirection(plane.Normal);
if (swapped)
{
normal = -normal;
}
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, contactSet.HaveContact);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
public PickingSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
SampleFramework.IsMouseVisible = false;
GraphicsScreen.ClearBackground = true;
GraphicsScreen.BackgroundColor = Color.CornflowerBlue;
GraphicsScreen.DrawReticle = true;
SetCamera(new Vector3(0, 1, 10), 0, 0);
// ----- Initialize collision detection system.
// We use one collision domain that manages all objects.
_domain = new CollisionDomain
{
// Optional: Change the broad phase type. The default type is the SweepAndPruneSpace,
// which is very fast for physics simulation. The DualPartition is better for ray casts.
// See also http://digitalrune.github.io/DigitalRune-Documentation/html/e32cab3b-cc7c-42ee-8ec9-23dd4467edd0.htm#WhichPartition
BroadPhase = new DualPartition <CollisionObject>(),
};
// Optional: Set a broad phase filter.
// Per default, the collision domain computes contacts between all collision objects. If we
// are only interested in ray vs non-ray-shape contacts, we can set a filter to avoid
// unnecessary intersection computations and improve performance.
_domain.BroadPhase.Filter = new DelegatePairFilter <CollisionObject>(
pair =>
{
var firstIsRay = pair.First.GeometricObject.Shape is RayShape;
var secondIsRay = pair.Second.GeometricObject.Shape is RayShape;
return(firstIsRay != secondIsRay);
});
// Create a collision object with a box shape at position (0, 0, 0) with a random rotation.
_box = new CollisionObject(
new GeometricObject(
new BoxShape(1, 2, 3),
new Pose(new Vector3(0, 0, 0), RandomHelper.Random.NextQuaternion())));
// Create a collision object with a sphere shape at position (-5, 0, 0).
_sphere = new CollisionObject(new GeometricObject(new SphereShape(1), new Pose(new Vector3(-5, 0, 0))));
// Create a random list of points.
var points = new List <Vector3>();
for (int i = 0; i < 100; i++)
{
points.Add(RandomHelper.Random.NextVector3(-1.5f, 1.5f));
}
// Create a triangle mesh of the convex hull.
// (See also the ConvexHullSample for info on convex hull creation.)
TriangleMesh triangleMesh = GeometryHelper.CreateConvexHull(points).ToTriangleMesh();
// We use this random triangle mesh to define a shape.
TriangleMeshShape meshShape = new TriangleMeshShape(triangleMesh);
// Optional: We can use a spatial partitioning method, to speed up collision
// detection for large meshes. AABB trees are good for static triangle meshes.
// To use spatial partitioning we have to set a valid spatial partition instance
// in the Partition property.
// The spatial partition will store indices of the mesh triangles, therefore
// the generic type argument is "int".
meshShape.Partition = new AabbTree <int>()
{
// Optional: The tree is automatically built using a mixed top-down/bottom-up approach.
// Bottom-up building is slower but produces better trees. If the tree building takes too
// long, we can lower the BottomUpBuildThreshold (default is 128).
BottomUpBuildThreshold = 0,
};
// Optional: Build the AABB tree. (This is done automatically when the AABB tree is used for
// the first time, but Update can also be called explicitly to control when the tree is built.)
meshShape.Partition.Update(false);
// Create a collision object with the random triangle mesh shape.
_mesh = new CollisionObject(new GeometricObject(meshShape, new Pose(new Vector3(5, 0, 0))));
// Add collision object to collision domain.
_domain.CollisionObjects.Add(_box);
_domain.CollisionObjects.Add(_sphere);
_domain.CollisionObjects.Add(_mesh);
// For picking we create a ray.
// The ray shoot from its local origin in +x direction.
// (Note: The last parameter is the length of the ray. In theory, rays have
// an infinite length. However, in the collision detection we use rays with
// a finite length. This increases the performance and improves the numerical
// stability of the algorithms.)
RayShape rayShape = new RayShape(Vector3.Zero, Vector3.Forward, 1000);
_ray = new CollisionObject(new GeometricObject(rayShape, Pose.Identity));
// The ray is just one additional collision object in our collision domain.
_domain.CollisionObjects.Add(_ray);
// The collision domain manages now 4 objects: a box, a sphere, a triangle mesh and a ray.
}
/// <summary>
/// Allows the user to drag a rigid body by using touch.
/// </summary>
private void DragBodies()
{
// Here is how it works:
// We first make a hit-test using a ray to check whether the user touches a rigid body.
// If there is a hit we create a spring (using a ball-socket joint) and connect the rigid
// body to the touch location. Every time the user moves her finger we update the position
// of the spring and the spring pulls the rigid body towards the finger.
// We use raw touch points to select and drag a rigid body.
TouchCollection touches = InputService.TouchCollection;
if (touches.Count == 0)
{
// No touches detected.
if (_spring != null)
{
// There is an active spring, so the user is currently dragging a rigid body.
// Release the body by removing the spring.
_spring.Simulation.Constraints.Remove(_spring);
_spring = null;
}
}
else
{
// Touch detected.
TouchLocation touchLocation = touches[0];
// Convert the touch location from screen coordinates to world coordinates.
var cameraNode = GraphicsScreen.CameraNode;
Vector3 pScreen = new Vector3(touchLocation.Position.X, touchLocation.Position.Y, 0);
Vector3 pWorld = GraphicsService.GraphicsDevice.Viewport.Unproject(
pScreen,
cameraNode.Camera.Projection,
(Matrix)cameraNode.View,
Matrix.Identity);
// pWorld is point on the near clip plane of the camera.
// Set the origin and direction of the ray for hit-testing.
Vector3F rayOrigin = _cameraPosition;
Vector3F rayDirection = ((Vector3F)pWorld - _cameraPosition).Normalized;
if (touchLocation.State == TouchLocationState.Pressed)
{
// Let's create a ray and see if we hit a rigid body.
// (Create the ray shape and the required collision object only once.)
if (_rayShape == null)
{
_rayShape = new RayShape {
StopsAtFirstHit = true
};
_rayCollisionObject = new CollisionObject(new GeometricObject(_rayShape));
}
// Set the origin and direction of the ray.
_rayShape.Origin = rayOrigin;
_rayShape.Direction = rayDirection.Normalized;
// Make a hit test using the collision detection and get the first contact found.
var contactSet = Simulation.CollisionDomain
.GetContacts(_rayCollisionObject)
.FirstOrDefault();
if (contactSet != null && contactSet.Count > 0)
{
// Get the point where the ray hits the rigid body.
Contact contact = contactSet[0];
// The contact sets contains two objects ("ObjectA" and "ObjectB").
// One is the ray the other is the object that was hit by the ray.
var hitCollisionObject = (contactSet.ObjectA == _rayCollisionObject)
? contactSet.ObjectB
: contactSet.ObjectA;
// Check whether the object is a dynamic rigid body.
var hitBody = hitCollisionObject.GeometricObject as RigidBody;
if (hitBody != null && hitBody.MotionType == MotionType.Dynamic)
{
// Remove the old joint, in case a rigid body is already grabbed.
if (_spring != null && _spring.Simulation != null)
{
_spring.Simulation.Constraints.Remove(_spring);
}
// The penetration depth tells us the distance from the ray origin to the rigid body
// in view direction.
_springAnchorDistanceFromCamera = contact.PenetrationDepth;
// Get the position where the ray hits the other object.
// (The position is defined in the local space of the object.)
Vector3F hitPositionLocal = (contactSet.ObjectA == _rayCollisionObject)
? contact.PositionBLocal
: contact.PositionALocal;
// Attach the rigid body at the touch location using a ball-socket joint.
// (Note: We could also use a FixedJoint, if we don't want any rotations.)
_spring = new BallJoint
{
BodyA = hitBody,
AnchorPositionALocal = hitPositionLocal,
// We need to attach the grabbed object to a second body. In this case we just want to
// anchor the object at a specific point in the world. To achieve this we can use the
// special rigid body "World", which is defined in the simulation.
BodyB = Simulation.World,
AnchorPositionBLocal = rayOrigin + rayDirection * _springAnchorDistanceFromCamera,
// Some constraint adjustments.
ErrorReduction = 0.3f,
// We set a softness > 0. This makes the joint "soft" and it will act like
// damped spring.
Softness = 0.00001f,
// We limit the maximal force. This reduces the ability of this joint to violate
// other constraints.
MaxForce = 1e6f
};
// Add the spring to the simulation.
Simulation.Constraints.Add(_spring);
}
}
}
else if (touchLocation.State == TouchLocationState.Moved)
{
if (_spring != null)
{
// User has grabbed something.
// Update the position of the object by updating the anchor position of the ball-socket
// joint.
_spring.AnchorPositionBLocal = rayOrigin + rayDirection * _springAnchorDistanceFromCamera;
// Reduce the angular velocity by a certain factor. (This acts like a damping because we
// do not want the object to rotate like crazy.)
_spring.BodyA.AngularVelocity *= 0.9f;
}
}
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Ray vs. convex has at max 1 contact.
Debug.Assert(contactSet.Count <= 1);
// Object A should be the ray.
// Object B should be the convex.
IGeometricObject rayObject = contactSet.ObjectA.GeometricObject;
IGeometricObject convexObject = contactSet.ObjectB.GeometricObject;
// Swap object if necessary.
bool swapped = (convexObject.Shape is RayShape);
if (swapped)
{
MathHelper.Swap(ref rayObject, ref convexObject);
}
RayShape rayShape = rayObject.Shape as RayShape;
ConvexShape convexShape = convexObject.Shape as ConvexShape;
// Check if shapes are correct.
if (rayShape == null || convexShape == null)
{
throw new ArgumentException("The contact set must contain a ray and a convex shape.", "contactSet");
}
// Call line segment vs. convex for closest points queries.
if (type == CollisionQueryType.ClosestPoints)
{
// Find point on ray closest to the convex shape.
// Call GJK.
_gjk.ComputeCollision(contactSet, type);
if (contactSet.HaveContact == false)
{
return;
}
// Otherwise compute 1 contact ...
// GJK result is invalid for penetration.
foreach (var contact in contactSet)
{
contact.Recycle();
}
contactSet.Clear();
}
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3 rayScale = rayObject.Scale;
Vector3 convexScale = convexObject.Scale;
Pose convexPose = convexObject.Pose;
Pose rayPose = rayObject.Pose;
// See Raycasting paper of van den Bergen or Bullet.
// Note: Compute in local space of convex (object B).
// Scale ray and transform ray to local space of convex.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale); // Scale ray.
rayWorld.ToWorld(ref rayPose); // Transform ray to world space.
Ray ray = rayWorld;
ray.ToLocal(ref convexPose); // Transform ray to local space of convex.
var simplex = GjkSimplexSolver.Create();
try
{
Vector3 s = ray.Origin; // source
Vector3 r = ray.Direction * ray.Length; // ray
float λ = 0; // ray parameter
Vector3 x = s; // hit spot (on ray)
Vector3 n = new Vector3(); // normal
Vector3 v = x - convexShape.GetSupportPoint(ray.Direction, convexScale);
// v = x - arbitrary point. Vector used for support mapping.
float distanceSquared = v.LengthSquared(); // ||v||²
int iterationCount = 0;
while (distanceSquared > Numeric.EpsilonF && iterationCount < MaxNumberOfIterations)
{
iterationCount++;
Vector3 p = convexShape.GetSupportPoint(v, convexScale); // point on convex
Vector3 w = x - p; // simplex/Minkowski difference point
float vDotW = Vector3.Dot(v, w); // v∙w
if (vDotW > 0)
{
float vDotR = Vector3.Dot(v, r); // v∙r
if (vDotR >= 0) // TODO: vDotR >= - Epsilon^2 ?
{
return; // No Hit.
}
λ = λ - vDotW / vDotR;
x = s + λ * r;
simplex.Clear(); // Configuration space obstacle (CSO) is translated whenever x is updated.
w = x - p;
n = v;
}
simplex.Add(w, x, p);
simplex.Update();
v = simplex.ClosestPoint;
distanceSquared = (simplex.IsValid && !simplex.IsFull) ? v.LengthSquared() : 0;
}
// We have a contact if the hit is inside the ray length.
contactSet.HaveContact = (0 <= λ && λ <= 1);
if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
{
// HaveContact queries can exit here.
// GetContacts queries can exit here if we don't have a contact.
return;
}
float penetrationDepth = λ * ray.Length;
Debug.Assert(contactSet.HaveContact, "Separation was not detected by GJK above.");
// Convert back to world space.
Vector3 position = rayWorld.Origin + rayWorld.Direction * penetrationDepth;
n = convexPose.ToWorldDirection(n);
if (!n.TryNormalize())
{
n = Vector3.UnitY;
}
if (swapped)
{
n = -n;
}
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, -n, penetrationDepth, true);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
finally
{
simplex.Recycle();
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
if (type == CollisionQueryType.ClosestPoints)
{
// Just use normal composite shape algorithm.
_compositeAlgorithm.ComputeCollision(contactSet, type);
return;
}
Debug.Assert(type != CollisionQueryType.ClosestPoints, "Closest point queries should have already been handled!");
// Composite = A, Ray = B
IGeometricObject compositeObject = contactSet.ObjectA.GeometricObject;
IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;
// Object A should be the composite, swap objects if necessary.
bool swapped = (compositeObject.Shape is RayShape);
if (swapped)
{
MathHelper.Swap(ref rayObject, ref compositeObject);
}
RayShape rayShape = rayObject.Shape as RayShape;
CompositeShape compositeShape = compositeObject.Shape as CompositeShape;
// Check if shapes are correct.
if (rayShape == null || compositeShape == null)
{
throw new ArgumentException("The contact set must contain a ray and a composite shape.", "contactSet");
}
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3F rayScale = rayObject.Scale;
Pose rayPose = rayObject.Pose;
Vector3F compositeScale = compositeObject.Scale;
Pose compositePose = compositeObject.Pose;
// Check if transforms are supported.
// Same check for object B.
if (compositeShape != null &&
(compositeScale.X != compositeScale.Y || compositeScale.Y != compositeScale.Z) &&
compositeShape.Children.Any(child => child.Pose.HasRotation)) // Note: Any() creates garbage, but non-uniform scalings should not be used anyway.
{
throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported.");
}
// ----- A few fixed objects which are reused to avoid GC garbage.
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
var testGeometricObject = TestGeometricObject.Create();
// Create a test contact set and initialize with dummy objects.
// (The actual collision objects are set below.)
var testContactSet = ContactSet.Create(testCollisionObject, contactSet.ObjectB); // Dummy arguments! They are changed later.
// Scale ray and transform ray to local unscaled space of composite.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale); // Scale ray.
rayWorld.ToWorld(ref rayPose); // Transform ray to world space.
Ray ray = rayWorld;
ray.ToLocal(ref compositePose); // Transform ray to local space of composite.
var inverseCompositeScale = Vector3F.One / compositeScale;
ray.Scale(ref inverseCompositeScale);
try
{
if (compositeShape.Partition != null)
{
#region ----- Composite with BVH vs. * -----
foreach (var childIndex in compositeShape.Partition.GetOverlaps(ray))
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
{
break; // We can abort early.
}
AddChildContacts(
contactSet,
swapped,
childIndex,
type,
testContactSet,
testCollisionObject,
testGeometricObject);
}
#endregion
}
else
{
#region ----- Composite vs. *-----
var rayDirectionInverse = new Vector3F(
1 / ray.Direction.X,
1 / ray.Direction.Y,
1 / ray.Direction.Z);
float epsilon = Numeric.EpsilonF * (1 + compositeObject.Aabb.Extent.Length);
// Go through list of children and find contacts.
int numberOfChildGeometries = compositeShape.Children.Count;
for (int i = 0; i < numberOfChildGeometries; i++)
{
IGeometricObject child = compositeShape.Children[i];
if (GeometryHelper.HaveContact(child.Shape.GetAabb(child.Scale, child.Pose), ray.Origin, rayDirectionInverse, ray.Length, epsilon))
{
AddChildContacts(
contactSet,
swapped,
i,
type,
testContactSet,
testCollisionObject,
testGeometricObject);
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
{
break;
}
}
}
#endregion
}
}
finally
{
Debug.Assert(compositeObject.Shape == compositeShape, "Shape was altered and not restored.");
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
}
}
// OnUpdate() is called once per frame.
protected override void OnUpdate(TimeSpan deltaTime)
{
if (_spring != null
&& !_inputService.IsDown(MouseButtons.Left)
&& !_inputService.IsDown(Buttons.LeftTrigger, LogicalPlayerIndex.One))
{
// The user has released the object.
_simulation.Constraints.Remove(_spring);
_spring = null;
}
if (!_inputService.IsMouseOrTouchHandled
&& !_inputService.IsGamePadHandled(LogicalPlayerIndex.Any)
&& (_inputService.IsPressed(MouseButtons.Left, false)
|| _inputService.IsPressed(Buttons.LeftTrigger, false, LogicalPlayerIndex.One)))
{
// The user has pressed the grab button and the input was not already handled
// by another game object.
// Remove the old joint, in case anything is grabbed.
if (_spring != null)
{
_simulation.Constraints.Remove(_spring);
_spring = null;
}
// The spring is attached at the position that is targeted with the cross-hair.
// We can perform a ray hit-test to find the position. The ray starts at the camera
// position and shoots forward (-z direction).
var cameraGameObject = (CameraObject)_gameObjectService.Objects["Camera"];
var cameraNode = cameraGameObject.CameraNode;
Vector3F cameraPosition = cameraNode.PoseWorld.Position;
Vector3F cameraDirection = cameraNode.PoseWorld.ToWorldDirection(Vector3F.Forward);
// Create a ray for picking.
RayShape ray = new RayShape(cameraPosition, cameraDirection, 1000);
// The ray should stop at the first hit. We only want the first object.
ray.StopsAtFirstHit = true;
// The collision detection requires a CollisionObject.
CollisionObject rayCollisionObject = new CollisionObject(new GeometricObject(ray, Pose.Identity));
// Assign the collision object to collision group 2. (In SampleGame.cs a
// collision filter based on collision groups was set. Objects for hit-testing
// are in group 2.)
rayCollisionObject.CollisionGroup = 2;
// Get the first object that has contact with the ray.
ContactSet contactSet = _simulation.CollisionDomain.GetContacts(rayCollisionObject).FirstOrDefault();
if (contactSet != null && contactSet.Count > 0)
{
// The ray has hit something.
// The contact set contains all detected contacts between the ray and the rigid body.
// Get the first contact in the contact set. (A ray hit usually contains exactly 1 contact.)
Contact contact = contactSet[0];
// The contact set contains the object pair of the collision. One object is the ray.
// The other is the object we want to grab.
CollisionObject hitCollisionObject = (contactSet.ObjectA == rayCollisionObject) ? contactSet.ObjectB : contactSet.ObjectA;
// Check whether a dynamic rigid body was hit.
RigidBody hitBody = hitCollisionObject.GeometricObject as RigidBody;
if (hitBody != null && hitBody.MotionType == MotionType.Dynamic)
{
// Attach the rigid body at the cursor position using a ball-socket joint.
// (Note: We could also use a FixedJoint, if we don't want any rotations.)
// The penetration depth tells us the distance from the ray origin to the rigid body.
_springAttachmentDistanceFromObserver = contact.PenetrationDepth;
// Get the position where the ray hits the other object.
// (The position is defined in the local space of the object.)
Vector3F hitPositionLocal = (contactSet.ObjectA == rayCollisionObject) ? contact.PositionBLocal : contact.PositionALocal;
_spring = new BallJoint
{
BodyA = hitBody,
AnchorPositionALocal = hitPositionLocal,
// We need to attach the grabbed object to a second body. In this case we just want to
// anchor the object at a specific point in the world. To achieve this we can use the
// special rigid body "World", which is defined in the simulation.
BodyB = _simulation.World,
// AnchorPositionBLocal is set below.
// Some constraint adjustments.
ErrorReduction = 0.3f,
// We set a softness > 0. This makes the joint "soft" and it will act like
// damped spring.
Softness = 0.00001f,
// We limit the maximal force. This reduces the ability of this joint to violate
// other constraints.
MaxForce = 1e6f
};
// Add the spring to the simulation.
_simulation.Constraints.Add(_spring);
}
}
}
if (_spring != null)
{
// User has grabbed something.
// Update the position of the object by updating the anchor position of
// the ball-socket joint.
var cameraGameObject = (CameraObject)_gameObjectService.Objects["Camera"];
var cameraNode = cameraGameObject.CameraNode;
Vector3F cameraPosition = cameraNode.PoseWorld.Position;
Vector3F cameraDirection = cameraNode.PoseWorld.ToWorldDirection(-Vector3F.UnitZ);
_spring.AnchorPositionBLocal = cameraPosition + cameraDirection * _springAttachmentDistanceFromObserver;
// Reduce the angular velocity by a certain factor. (This acts like a damping because we
// do not want the object to rotate like crazy.)
_spring.BodyA.AngularVelocity *= 0.9f;
}
}
public void TestProperties()
{
RayShape l = new RayShape();
Assert.AreEqual(new Vector3F(), l.Origin);
Assert.AreEqual(new Vector3F(1, 0, 0), l.Direction);
l.Origin = new Vector3F(1, 2, 3);
Assert.AreEqual(new Vector3F(1, 2, 3), l.Origin);
Assert.AreEqual(new Vector3F(1, 0, 0), l.Direction);
l.Direction = new Vector3F(4, 5, 6).Normalized;
Assert.AreEqual(new Vector3F(1, 2, 3), l.Origin);
Assert.AreEqual(new Vector3F(4, 5, 6).Normalized, l.Direction);
l.Length = 11;
Assert.AreEqual(new Vector3F(1, 2, 3), l.Origin);
Assert.AreEqual(new Vector3F(4, 5, 6).Normalized, l.Direction);
Assert.AreEqual(11, l.Length);
Assert.AreEqual(false, l.StopsAtFirstHit);
l.StopsAtFirstHit = true;
Assert.AreEqual(new Vector3F(1, 2, 3), l.Origin);
Assert.AreEqual(new Vector3F(4, 5, 6).Normalized, l.Direction);
Assert.AreEqual(11, l.Length);
Assert.AreEqual(true, l.StopsAtFirstHit);
}
// Creates a lot of random objects.
private void CreateRandomObjects()
{
var random = new Random();
var isFirstHeightField = true;
int currentShape = 0;
int numberOfObjects = 0;
while (true)
{
numberOfObjects++;
if (numberOfObjects > ObjectsPerType)
{
currentShape++;
numberOfObjects = 0;
}
Shape shape;
switch (currentShape)
{
case 0:
// Box
shape = new BoxShape(ObjectSize, ObjectSize * 2, ObjectSize * 3);
break;
case 1:
// Capsule
shape = new CapsuleShape(0.3f * ObjectSize, 2 * ObjectSize);
break;
case 2:
// Cone
shape = new ConeShape(1 * ObjectSize, 2 * ObjectSize);
break;
case 3:
// Cylinder
shape = new CylinderShape(0.4f * ObjectSize, 2 * ObjectSize);
break;
case 4:
// Sphere
shape = new SphereShape(ObjectSize);
break;
case 5:
// Convex hull of several points.
ConvexHullOfPoints hull = new ConvexHullOfPoints();
hull.Points.Add(new Vector3(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize));
hull.Points.Add(new Vector3(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize));
hull.Points.Add(new Vector3(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
hull.Points.Add(new Vector3(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
hull.Points.Add(new Vector3(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize));
shape = hull;
break;
case 6:
// A composite shape: two boxes that form a "T" shape.
var composite = new CompositeShape();
composite.Children.Add(
new GeometricObject(
new BoxShape(ObjectSize, 3 * ObjectSize, ObjectSize),
new Pose(new Vector3(0, 0, 0))));
composite.Children.Add(
new GeometricObject(
new BoxShape(2 * ObjectSize, ObjectSize, ObjectSize),
new Pose(new Vector3(0, 2 * ObjectSize, 0))));
shape = composite;
break;
case 7:
shape = new CircleShape(ObjectSize);
break;
case 8:
{
var compBvh = new CompositeShape();
compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3(0, 0.5f, 0), Matrix.Identity)));
compBvh.Children.Add(new GeometricObject(new BoxShape(0.8f, 0.5f, 0.5f), new Pose(new Vector3(0.5f, 0.7f, 0), Matrix.CreateRotationZ(-MathHelper.ToRadians(15)))));
compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3(0, 1.15f, 0), Matrix.Identity)));
compBvh.Children.Add(new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3(0.6f, 1.15f, 0), Matrix.CreateRotationX(0.3f))));
compBvh.Partition = new AabbTree<int>();
shape = compBvh;
break;
}
case 9:
CompositeShape comp = new CompositeShape();
comp.Children.Add(new GeometricObject(new BoxShape(0.5f * ObjectSize, 1 * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3(0, 0.5f * ObjectSize, 0), Quaternion.Identity)));
comp.Children.Add(new GeometricObject(new BoxShape(0.8f * ObjectSize, 0.5f * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3(0.3f * ObjectSize, 0.7f * ObjectSize, 0), Quaternion.CreateRotationZ(-MathHelper.ToRadians(45)))));
comp.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3(0, 1.15f * ObjectSize, 0), Quaternion.Identity)));
shape = comp;
break;
case 10:
shape = new ConvexHullOfPoints(new[]
{
new Vector3(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize),
new Vector3(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize),
new Vector3(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize)
});
break;
case 11:
ConvexHullOfShapes shapeHull = new ConvexHullOfShapes();
shapeHull.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3(0, 2 * ObjectSize, 0), Matrix.Identity)));
shapeHull.Children.Add(new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize), Pose.Identity));
shape = shapeHull;
break;
case 12:
shape = Shape.Empty;
break;
case 13:
var numberOfSamplesX = 10;
var numberOfSamplesZ = 10;
var samples = new float[numberOfSamplesX * numberOfSamplesZ];
for (int z = 0; z < numberOfSamplesZ; z++)
for (int x = 0; x < numberOfSamplesX; x++)
samples[z * numberOfSamplesX + x] = (float)(Math.Cos(z / 3f) * Math.Sin(x / 2f) * BoxSize / 6);
HeightField heightField = new HeightField(0, 0, 2 * BoxSize, 2 * BoxSize, samples, numberOfSamplesX, numberOfSamplesZ);
shape = heightField;
break;
//case 14:
//shape = new LineShape(new Vector3(0.1f, 0.2f, 0.3f), new Vector3(0.1f, 0.2f, -0.3f).Normalized);
//break;
case 15:
shape = new LineSegmentShape(
new Vector3(0.1f, 0.2f, 0.3f), new Vector3(0.1f, 0.2f, 0.3f) + 3 * ObjectSize * new Vector3(0.1f, 0.2f, -0.3f));
break;
case 16:
shape = new MinkowskiDifferenceShape
{
ObjectA = new GeometricObject(new SphereShape(0.1f * ObjectSize)),
ObjectB = new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize))
};
break;
case 17:
shape = new MinkowskiSumShape
{
ObjectA = new GeometricObject(new SphereShape(0.1f * ObjectSize)),
ObjectB = new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize)),
};
break;
case 18:
shape = new OrthographicViewVolume(0, ObjectSize, 0, ObjectSize, ObjectSize / 2, ObjectSize * 2);
break;
case 19:
shape = new PerspectiveViewVolume(MathHelper.ToRadians(60f), 16f / 10, ObjectSize / 2, ObjectSize * 3);
break;
case 20:
shape = new PointShape(0.1f, 0.3f, 0.2f);
break;
case 21:
shape = new RayShape(new Vector3(0.2f, 0, -0.12f), new Vector3(1, 2, 3).Normalized, ObjectSize * 2);
break;
case 22:
shape = new RayShape(new Vector3(0.2f, 0, -0.12f), new Vector3(1, 2, 3).Normalized, ObjectSize * 2)
{
StopsAtFirstHit = true
};
break;
case 23:
shape = new RectangleShape(ObjectSize, ObjectSize * 2);
break;
case 24:
shape = new TransformedShape(
new GeometricObject(
new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize),
new Pose(new Vector3(0.1f, 1, -0.2f))));
break;
case 25:
shape = new TriangleShape(
new Vector3(ObjectSize, 0, 0), new Vector3(0, ObjectSize, 0), new Vector3(ObjectSize, ObjectSize, ObjectSize));
break;
//case 26:
// {
// // Create a composite object from which we get the mesh.
// CompositeShape compBvh = new CompositeShape();
// compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3(0, 0.5f, 0), Matrix.Identity)));
// compBvh.Children.Add(
// new GeometricObject(
// new BoxShape(0.8f, 0.5f, 0.5f),
// new Pose(new Vector3(0.5f, 0.7f, 0), Matrix.CreateRotationZ(-(float)MathHelper.ToRadians(15)))));
// compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3(0, 1.15f, 0), Matrix.Identity)));
// compBvh.Children.Add(
// new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3(0.6f, 1.15f, 0), Matrix.CreateRotationX(0.3f))));
// TriangleMeshShape meshBvhShape = new TriangleMeshShape { Mesh = compBvh.GetMesh(0.01f, 3) };
// meshBvhShape.Partition = new AabbTree<int>();
// shape = meshBvhShape;
// break;
// }
//case 27:
// {
// // Create a composite object from which we get the mesh.
// CompositeShape compBvh = new CompositeShape();
// compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3(0, 0.5f, 0), Quaternion.Identity)));
// compBvh.Children.Add(
// new GeometricObject(
// new BoxShape(0.8f, 0.5f, 0.5f),
// new Pose(new Vector3(0.5f, 0.7f, 0), Quaternion.CreateRotationZ(-(float)MathHelper.ToRadians(15)))));
// compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3(0, 1.15f, 0), Quaternion.Identity)));
// compBvh.Children.Add(
// new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3(0.6f, 1.15f, 0), Quaternion.CreateRotationX(0.3f))));
// TriangleMeshShape meshBvhShape = new TriangleMeshShape { Mesh = compBvh.GetMesh(0.01f, 3) };
// meshBvhShape.Partition = new AabbTree<int>();
// shape = meshBvhShape;
// break;
// }
case 28:
shape = new ConvexPolyhedron(new[]
{
new Vector3(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize),
new Vector3(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize),
new Vector3(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize)
});
break;
case 29:
return;
default:
currentShape++;
continue;
}
// Create an object with the random shape, pose, color and velocity.
Pose randomPose = new Pose(
random.NextVector3(-BoxSize + ObjectSize * 2, BoxSize - ObjectSize * 2),
random.NextQuaternion());
var newObject = new MovingGeometricObject
{
Pose = randomPose,
Shape = shape,
LinearVelocity = random.NextQuaternion().Rotate(new Vector3(MaxLinearVelocity, 0, 0)),
AngularVelocity = random.NextQuaternion().Rotate(Vector3.Forward)
* RandomHelper.Random.NextFloat(0, MaxAngularVelocity),
};
if (RandomHelper.Random.NextBool())
newObject.LinearVelocity = Vector3.Zero;
if (RandomHelper.Random.NextBool())
newObject.AngularVelocity = Vector3.Zero;
if (shape is LineShape || shape is HeightField)
{
// Do not move lines or the height field.
newObject.LinearVelocity = Vector3.Zero;
newObject.AngularVelocity = Vector3.Zero;
}
// Create only 1 heightField!
if (shape is HeightField)
{
if (isFirstHeightField)
{
isFirstHeightField = true;
newObject.Pose = new Pose(new Vector3(-BoxSize, -BoxSize, -BoxSize));
}
else
{
currentShape++;
numberOfObjects = 0;
continue;
}
}
// Add collision object to collision domain.
_domain.CollisionObjects.Add(new CollisionObject(newObject));
//co.Type = CollisionObjectType.Trigger;
//co.Name = "Object" + shape.GetType().Name + "_" + i;
}
}
public void SerializationXml()
{
var a = new RayShape(new Vector3F(1, 2, 3), new Vector3F(2, 3, 4).Normalized, 1234.567f);
a.StopsAtFirstHit = true;
// Serialize object.
var stream = new MemoryStream();
var serializer = new XmlSerializer(typeof(Shape));
serializer.Serialize(stream, a);
// Output generated xml. Can be manually checked in output window.
stream.Position = 0;
var xml = new StreamReader(stream).ReadToEnd();
Trace.WriteLine("Serialized Object:\n" + xml);
// Deserialize object.
stream.Position = 0;
var deserializer = new XmlSerializer(typeof(Shape));
var b = (RayShape)deserializer.Deserialize(stream);
Assert.AreEqual(a.Direction, b.Direction);
Assert.AreEqual(a.Length, b.Length);
Assert.AreEqual(a.Origin, b.Origin);
Assert.AreEqual(a.StopsAtFirstHit, b.StopsAtFirstHit);
}
public void SetData(RayShape shape)
{
currentShape = shape;
slider.value = shape.blendStrength;
}
public void GetSupportPoint()
{
RayShape r = new RayShape(new Vector3F(1, 0, 0), new Vector3F(1, 1, 0).Normalized, 10);
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(1, 0, 0), r.GetSupportPointNormalized(new Vector3F(-1, 0, 0))));
Assert.IsTrue(Vector3F.AreNumericallyEqual(r.Origin + r.Direction * r.Length, r.GetSupportPointNormalized(new Vector3F(1, 0, 0))));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(1, 0, 0), r.GetSupportPoint(new Vector3F(-2, 0, 0))));
Assert.IsTrue(Vector3F.AreNumericallyEqual(r.Origin + r.Direction * r.Length, r.GetSupportPoint(new Vector3F(2, 0, 0))));
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Object A should be the ray.
// Object B should be the triangle.
IGeometricObject rayObject = contactSet.ObjectA.GeometricObject;
IGeometricObject triangleObject = contactSet.ObjectB.GeometricObject;
// Swap if necessary.
bool swapped = (triangleObject.Shape is RayShape);
if (swapped)
{
MathHelper.Swap(ref rayObject, ref triangleObject);
}
RayShape rayShape = rayObject.Shape as RayShape;
TriangleShape triangleShape = triangleObject.Shape as TriangleShape;
// Check if shapes are correct.
if (rayShape == null || triangleShape == null)
{
throw new ArgumentException("The contact set must contain a ray and a triangle.", "contactSet");
}
// See SOLID and Bergen: "Collision Detection in Interactive 3D Environments", pp. 84.
// Note: All computations are done in triangle local space.
// Get transformations.
Vector3F rayScale = rayObject.Scale;
Vector3F triangleScale = triangleObject.Scale;
Pose rayPose = rayObject.Pose;
Pose trianglePose = triangleObject.Pose;
// Scale triangle.
Vector3F v0 = triangleShape.Vertex0 * triangleScale;
Vector3F v1 = triangleShape.Vertex1 * triangleScale;
Vector3F v2 = triangleShape.Vertex2 * triangleScale;
// Scale ray and transform ray to local space of triangle.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale); // Scale ray.
rayWorld.ToWorld(ref rayPose); // Transform to world space.
Ray ray = rayWorld;
ray.ToLocal(ref trianglePose); // Transform to local space of triangle.
Vector3F d1 = (v1 - v0);
Vector3F d2 = (v2 - v0);
Vector3F n = Vector3F.Cross(d1, d2);
// Tolerance value, see SOLID, Bergen: "Collision Detection in Interactive 3D Environments".
float ε = n.Length * Numeric.EpsilonFSquared;
Vector3F r = ray.Direction * ray.Length;
float δ = -Vector3F.Dot(r, n);
if (ε == 0.0f || Numeric.IsZero(δ, ε))
{
// The triangle is degenerate or the ray is parallel to triangle.
if (type == CollisionQueryType.Contacts || type == CollisionQueryType.Boolean)
{
contactSet.HaveContact = false;
}
else if (type == CollisionQueryType.ClosestPoints)
{
GetClosestPoints(contactSet, rayWorld.Origin);
}
return;
}
Vector3F triangleToRayOrigin = ray.Origin - v0;
float λ = Vector3F.Dot(triangleToRayOrigin, n) / δ;
// Assume no contact.
contactSet.HaveContact = false;
if (λ < 0 || λ > 1)
{
// The ray does not hit.
if (type == CollisionQueryType.ClosestPoints)
{
GetClosestPoints(contactSet, rayWorld.Origin);
}
}
else
{
Vector3F u = Vector3F.Cross(triangleToRayOrigin, r);
float μ1 = Vector3F.Dot(d2, u) / δ;
float μ2 = Vector3F.Dot(-d1, u) / δ;
if (μ1 + μ2 <= 1 + ε && μ1 >= -ε && μ2 >= -ε)
{
// Hit!
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
{
return;
}
float penetrationDepth = λ * ray.Length;
// Create contact info.
Vector3F position = rayWorld.Origin + rayWorld.Direction * penetrationDepth;
n = trianglePose.ToWorldDirection(n);
Debug.Assert(!n.IsNumericallyZero, "Degenerate cases of ray vs. triangle should be treated above.");
n.Normalize();
if (δ > 0)
{
n = -n;
}
if (swapped)
{
n = -n;
}
Contact contact = ContactHelper.CreateContact(contactSet, position, n, penetrationDepth, true);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
else
{
// No Hit!
if (type == CollisionQueryType.ClosestPoints)
{
GetClosestPoints(contactSet, rayWorld.Origin);
}
}
}
}
/// <summary>
/// Updates the contact geometry for a single contact.
/// </summary>
/// <param name="contactSet">The contact set.</param>
/// <param name="contact">The contact to be updated.</param>
/// <param name="contactPositionTolerance">The contact position tolerance.</param>
/// <returns>
/// <see langword="true"/> if the contact is invalid and should be removed.
/// </returns>
private static bool UpdateContact(ContactSet contactSet, Contact contact, float contactPositionTolerance)
{
Pose poseA = contactSet.ObjectA.GeometricObject.Pose;
Pose poseB = contactSet.ObjectB.GeometricObject.Pose;
// Get local positions in world space.
//Vector3 positionA = poseA.ToWorldPosition(contact.PositionALocal);
//Vector3 positionB = poseB.ToWorldPosition(contact.PositionBLocal);
// ----- Optimized version:
Vector3 positionALocal = contact.PositionALocal;
Vector3 positionA;
positionA.X = poseA.Orientation.M00 * positionALocal.X + poseA.Orientation.M01 * positionALocal.Y + poseA.Orientation.M02 * positionALocal.Z + poseA.Position.X;
positionA.Y = poseA.Orientation.M10 * positionALocal.X + poseA.Orientation.M11 * positionALocal.Y + poseA.Orientation.M12 * positionALocal.Z + poseA.Position.Y;
positionA.Z = poseA.Orientation.M20 * positionALocal.X + poseA.Orientation.M21 * positionALocal.Y + poseA.Orientation.M22 * positionALocal.Z + poseA.Position.Z;
Vector3 positionBLocal = contact.PositionBLocal;
Vector3 positionB;
positionB.X = poseB.Orientation.M00 * positionBLocal.X + poseB.Orientation.M01 * positionBLocal.Y + poseB.Orientation.M02 * positionBLocal.Z + poseB.Position.X;
positionB.Y = poseB.Orientation.M10 * positionBLocal.X + poseB.Orientation.M11 * positionBLocal.Y + poseB.Orientation.M12 * positionBLocal.Z + poseB.Position.Y;
positionB.Z = poseB.Orientation.M20 * positionBLocal.X + poseB.Orientation.M21 * positionBLocal.Y + poseB.Orientation.M22 * positionBLocal.Z + poseB.Position.Z;
// Update Position.
contact.Position = (positionA + positionB) / 2;
// Update contacts and closest points differently:
if (contact.PenetrationDepth >= 0)
{
// ----- Contact.
Vector3 bToA = positionA - positionB; // Vector from contact on A to contact on B
if (!contact.IsRayHit)
{
// ----- Normal contact.
// Update penetration depth: Difference of world position projected onto normal.
//contact.PenetrationDepth = Vector3.Dot(bToA, contact.Normal);
// ----- Optimized version:
Vector3 contactNormal = contact.Normal;
contact.PenetrationDepth = bToA.X * contactNormal.X + bToA.Y * contactNormal.Y + bToA.Z * contactNormal.Z;
}
else
{
// ----- Ray hit.
// Update penetration depth: Contact position to ray origin projected onto ray direction.
// Get ray. Only one shape is a ray because ray vs. ray do normally not collide.
RayShape ray = contactSet.ObjectA.GeometricObject.Shape as RayShape;
float rayScale = contactSet.ObjectA.GeometricObject.Scale.X; // Non-uniformly scaled rays are not support, so we only need Scale.X!
Vector3 hitPositionLocal; // Hit position in local space of ray.
if (ray != null)
{
hitPositionLocal = poseA.ToLocalPosition(contact.Position);
}
else
{
// The other object must be the ray.
ray = contactSet.ObjectB.GeometricObject.Shape as RayShape;
rayScale = contactSet.ObjectB.GeometricObject.Scale.X; // Non-uniformly scaled rays are not support, so we only need Scale.X!
hitPositionLocal = poseB.ToLocalPosition(contact.Position);
}
// Now, we have found the ray, unless there is a composite shape with a child ray - which
// is not supported.
if (ray != null)
{
contact.PenetrationDepth = Vector3.Dot(hitPositionLocal - ray.Origin * rayScale, ray.Direction);
// If the new penetration depth is negative or greater than the ray length,
// the objects have separated along the ray direction.
if (contact.PenetrationDepth < 0 || contact.PenetrationDepth > ray.Length * rayScale)
{
return(true);
}
}
}
// Remove points with negative penetration depth.
if (contact.PenetrationDepth < 0)
{
return(true);
}
// Check drift.
float driftSquared;
if (contact.IsRayHit)
{
// For ray casts: Remove contact if movement in any direction is too large.
driftSquared = bToA.LengthSquared();
}
else
{
// For contacts: Remove contacts if horizontal movement (perpendicular to contact normal)
// is too large.
driftSquared = (bToA - contact.Normal * contact.PenetrationDepth).LengthSquared();
}
// Remove contact if drift is too large.
return(driftSquared > contactPositionTolerance * contactPositionTolerance);
}
else
{
// ----- Closest point pair.
// Update distance. Since we do not check the geometric objects, the new distance
// could be a separation or a penetration. We assume it is a separation and
// use a "-" sign.
// We have no problem if we are wrong and this is actually a penetration because this
// contact is automatically updated or removed when new contacts are computed in
// the narrow phase.
Vector3 aToB = positionB - positionA; // Vector from contact on A to contact on B
contact.PenetrationDepth = -aToB.Length;
// If points moved into contact, remove this pair, because we don't have a valid
// contact normal.
if (Numeric.IsZero(contact.PenetrationDepth))
{
return(true);
}
// Update normal.
contact.Normal = aToB.Normalized;
return(false);
}
}
