/// <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; }
// 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; }
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 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; } }
// 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 SetData(RayShape shape) { currentShape = shape; slider.value = shape.blendStrength; }
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); } }