/// <summary> /// Prepares the constraint for iterative processing in the current frame. /// </summary> public override void PreProcess() { RigidBody a = BodyA, b = BodyB; // get offsets and mass in world coordinates Vector3.Transform(ref _bodyPointA, ref a.World.Combined, out _worldOffsetA); Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA); Vector3.Transform(ref _bodyPointB, ref b.World.Combined, out _worldOffsetB); Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB); MassProperties.EffectiveMassMatrix(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, out _mass); if (this.Manager.IsSolverWarmStarted) { Vector3 impulse; Vector3.Multiply(ref _impulse, this.Manager.TimeStep, out impulse); b.ApplyImpulse(ref impulse, ref _worldOffsetB); Vector3.Negate(ref impulse, out impulse); a.ApplyImpulse(ref impulse, ref _worldOffsetA); } else { _impulse = Vector3.Zero; } }
/// <summary> /// Solve the constraint for position. /// </summary> /// <returns>Returns a value indicating whether the constraint has been satisfied.</returns> public override bool ProcessPosition() { RigidBody a = BodyA, b = BodyB; // get offsets and distance in world coordinates Vector3 impulse; Vector3.Transform(ref _bodyPointA, ref a.World.Combined, out _worldOffsetA); Vector3.Transform(ref _bodyPointB, ref b.World.Combined, out _worldOffsetB); Vector3.Subtract(ref _worldOffsetA, ref _worldOffsetB, out impulse); Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA); Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB); float error = impulse.Length(); if (error <= this.Manager.LinearErrorTolerance) { return(true); } // need normalized direction to calculate effective mass Vector3 n; Vector3.Divide(ref impulse, error, out n); float mass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, ref n); Vector3.Multiply(ref impulse, mass * this.Manager.PositionCorrectionFactor, out impulse); // apply impulse b.ApplyFlashImpulse(ref impulse, ref _worldOffsetB); Vector3.Negate(ref impulse, out impulse); a.ApplyFlashImpulse(ref impulse, ref _worldOffsetA); return(false); }
internal static float EffectiveMass(ref MassProperties a, ref MassProperties b, ref Vector3 offsetA, ref Vector3 offsetB, ref Vector3 normal) { Vector3 v; float normalMass = 0f, f; if (a.Mass < float.PositiveInfinity) { normalMass = a.MassInverse; Vector3.Cross(ref offsetA, ref normal, out v); Vector3.Transform(ref v, ref a.InertiaInverse, out v); Vector3.Cross(ref v, ref offsetA, out v); Vector3.Dot(ref normal, ref v, out f); normalMass += f; } if (b.Mass < float.PositiveInfinity) { normalMass += b.MassInverse; Vector3.Cross(ref offsetB, ref normal, out v); Vector3.Transform(ref v, ref b.InertiaInverse, out v); Vector3.Cross(ref v, ref offsetB, out v); Vector3.Dot(ref normal, ref v, out f); normalMass += f; } if (normalMass < Constants.Epsilon) { normalMass = Constants.Epsilon; } return(1f / normalMass); }
/// <summary> /// Prepares the constraint for iterative processing in the current frame. /// </summary> public override void PreProcess() { RigidBody a = BodyA, b = BodyB; // get world points and normal Vector3.Transform(ref _bodyPointA, ref a.World.Combined, out _worldOffsetA); Vector3.Transform(ref _bodyPointB, ref b.World.Combined, out _worldOffsetB); Vector3.Subtract(ref _worldOffsetA, ref _worldOffsetB, out _normal); Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA); Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB); _distance = _normal.Length(); if (_distance < Constants.Epsilon) { _normal = Vector3.Zero; } else { Vector3.Divide(ref _normal, _distance, out _normal); } _mass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, ref _normal); // determine the constraint behavior for this frame var prevState = _state; if (Math.Abs(_maxDistance - _minDistance) < 2f * this.Manager.LinearErrorTolerance) { _state = LimitState.Equal; } else if (_distance <= _minDistance) { _state = LimitState.Min; } else if (_distance >= _maxDistance) { _state = LimitState.Max; } else { _state = LimitState.Between; } if (this.Manager.IsSolverWarmStarted && prevState == _state) { Vector3 impulse; Vector3.Multiply(ref _impulse, this.Manager.TimeStep, out impulse); b.ApplyImpulse(ref impulse, ref _worldOffsetB); Vector3.Negate(ref impulse, out impulse); a.ApplyImpulse(ref impulse, ref _worldOffsetA); } else { _impulse = Vector3.Zero; } _pImpulse = Vector3.Zero; }
/// <summary> /// Construct a new rigid body. /// </summary> public RigidBody() { Mass = MassProperties.Immovable; Transform = World = Transform.Identity; _isMovable = false; _contacts = new List <Constraint>(); _constraints = new List <Constraint>(); _skin = new BodySkin(); _skin.Owner = this; }
public RigidBodyModel(MassProperties mass, CompiledPart[] parts, Material[] materials) { _mass = mass; _parts = parts; _materials = materials; if (_parts.Length != _materials.Length) { throw new ArgumentException("The count of supplied mesh parts and materials do not match."); } }
internal static void Transform(ref MassProperties mass, ref Transform transform, out MassProperties output) { output.Mass = mass.Mass; output.MassInverse = mass.MassInverse; Matrix orientation, orientationInv; Matrix.CreateFromQuaternion(ref transform.Orientation, out orientation); Matrix.Transpose(ref orientation, out orientationInv); Matrix.Multiply(ref orientationInv, ref mass.Inertia, out output.Inertia); Matrix.Multiply(ref output.Inertia, ref orientation, out output.Inertia); Matrix.Multiply(ref orientationInv, ref mass.InertiaInverse, out output.InertiaInverse); Matrix.Multiply(ref output.InertiaInverse, ref orientation, out output.InertiaInverse); }
internal static void EffectiveMassMatrix(ref MassProperties a, ref MassProperties b, ref Vector3 offsetA, ref Vector3 offsetB, out Matrix k) { Matrix ka = new Matrix(), kb = new Matrix(); if (a.Mass < float.PositiveInfinity) { a.InverseMassMatrix(ref offsetA, out ka); } if (b.Mass < float.PositiveInfinity) { b.InverseMassMatrix(ref offsetB, out kb); } Matrix.Add(ref ka, ref kb, out k); k.M44 = 1f; Matrix.Invert(ref k, out k); }
private void UpdateWorld() { World.Invert(out WorldInverse); MassProperties.Transform(ref Mass, ref World, out MassWorld); Transform = World; }
private void ComputeVitals() { RigidBody a = this.BodyA, b = this.BodyB; // compute relative basis between the two objects in world position Frame.Transform(ref _bodyBasisA, ref this.BodyA.World, out _worldBasisA); Frame.Transform(ref _bodyBasisB, ref this.BodyB.World, out _worldBasisB); Frame rel; Frame.Subtract(ref _worldBasisA, ref _worldBasisB, out rel); // compute current positions and angles Vector3.Subtract(ref _worldBasisA.Origin, ref _worldBasisB.Origin, out _positions); Matrix m; _worldBasisB.ToMatrix(out m); Matrix.Transpose(ref m, out m); Vector3.Transform(ref _positions, ref m, out _positions); rel.ComputeEulerAnglesXYZ(out _angles); // borrowed from Bullet; construct the axes about which we actually restrict rotation Vector3.Cross(ref _worldBasisB.Z, ref _worldBasisA.X, out _axisY); Vector3.Cross(ref _axisY, ref _worldBasisB.Z, out _axisX); Vector3.Cross(ref _worldBasisA.X, ref _axisY, out _axisZ); _axisX.Normalize(); _axisY.Normalize(); _axisZ.Normalize(); // calculate effective inertia along each axis Matrix inertia = new Matrix(); if (a.IsMovable) { Matrix.Add(ref inertia, ref a.MassWorld.InertiaInverse, out inertia); } if (b.IsMovable) { Matrix.Add(ref inertia, ref b.MassWorld.InertiaInverse, out inertia); } inertia.M44 = 1f; Matrix.Invert(ref inertia, out inertia); Vector3 d; Vector3.Transform(ref _axisX, ref inertia, out d); Vector3.Dot(ref d, ref _axisX, out _inertia.X); Vector3.Transform(ref _axisY, ref inertia, out d); Vector3.Dot(ref d, ref _axisY, out _inertia.Y); Vector3.Transform(ref _axisZ, ref inertia, out d); Vector3.Dot(ref d, ref _axisZ, out _inertia.Z); // calculate effective mass along each axis of basis B //float w = b.MassWorld.MassInverse / (a.MassWorld.MassInverse = b.MassWorld.MassInverse); //Vector3.Multiply(ref _worldBasisA.Origin, 1f - w, out _anchorA); //Vector3.Multiply(ref _worldBasisB.Origin, w, out _anchorB); //Vector3.Add(ref _anchorA, ref _anchorB, out _anchorB); //Vector3.Subtract(ref _anchorB, ref a.World.Position, out _anchorA); //Vector3.Subtract(ref _anchorB, ref b.World.Position, out _anchorB); Vector3.Subtract(ref _worldBasisA.Origin, ref a.World.Position, out _anchorA); Vector3.Subtract(ref _worldBasisB.Origin, ref b.World.Position, out _anchorB); _mass.X = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _anchorA, ref _anchorB, ref _worldBasisB.X); _mass.Y = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _anchorA, ref _anchorB, ref _worldBasisB.Y); _mass.Z = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _anchorA, ref _anchorB, ref _worldBasisB.Z); }
/// <summary> /// Construct a new rigid body. /// </summary> public RigidBody() { Mass = MassProperties.Immovable; Transform = World = Transform.Identity; _isMovable = false; _contacts = new List<Constraint>(); _constraints = new List<Constraint>(); _skin = new BodySkin(); _skin.Owner = this; }
public override ModelContent Process(NodeContent input, ContentProcessorContext context) { var attributes = input.Children.ToDictionary(n => n.Name, n => n.OpaqueData); var nodesToRemove = (from node in input.Children where node.OpaqueData.GetAttribute(TYPE_ATTR_NAME, MeshType.Both) == MeshType.Physical select node).ToArray(); ModelContent model = base.Process(input, context); var parts = new List<CompiledPart>(); var materials = new List<Material>(); var mass = new MassProperties(); var centerOfMass = Vector3.Zero; foreach (var mesh in model.Meshes) { MeshType type = MeshType.Both; PhysicalShape shape = PhysicalShape.Mesh; float elasticity = _defaultElasticity, roughness = _defaultRoughness, density = _defaultDensity; if (attributes.ContainsKey(mesh.Name)) { type = attributes[mesh.Name].GetAttribute(TYPE_ATTR_NAME, MeshType.Both); if (type == MeshType.Visual) continue; elasticity = attributes[mesh.Name].GetAttribute(ELASTICITY_ATTR_NAME, _defaultElasticity); roughness = attributes[mesh.Name].GetAttribute(ROUGHNESS_ATTR_NAME, _defaultRoughness); density = attributes[mesh.Name].GetAttribute(DENSITY_ATTR_NAME, _defaultDensity); shape = attributes[mesh.Name].GetAttribute(SHAPE_ATTR_NAME, _defaultShape); } var meshCenterOfMass = Vector3.Zero; var meshMass = MassProperties.Immovable; CompiledPart meshPart = null; if (mesh.MeshParts.Count < 1) { continue; } int[] indices = mesh.MeshParts[0].IndexBuffer.Skip(mesh.MeshParts[0].StartIndex).Take(mesh.MeshParts[0].PrimitiveCount * 3).ToArray(); Vector3[] vertices = MeshToVertexArray(context.TargetPlatform, mesh); if (_windingOrder == WindingOrder.Clockwise) { ReverseWindingOrder(indices); } switch (shape) { case PhysicalShape.Mesh: { meshPart = new CompiledMesh(vertices, indices); meshMass = MassProperties.Immovable; meshCenterOfMass = GetMeshTranslation(mesh); } break; case PhysicalShape.Polyhedron: { var hull = new ConvexHull3D(vertices); meshPart = hull.ToPolyhedron(); meshMass = MassProperties.FromTriMesh(density, vertices, indices, out meshCenterOfMass); } break; case PhysicalShape.Sphere: { Sphere s; Sphere.Fit(vertices, out s); meshPart = new CompiledSphere(s.Center, s.Radius); meshMass = MassProperties.FromSphere(density, s.Center, s.Radius); meshCenterOfMass = s.Center; } break; case PhysicalShape.Capsule: { Capsule c; Capsule.Fit(vertices, out c); meshPart = new CompiledCapsule(c.P1, c.P2, c.Radius); meshMass = MassProperties.FromCapsule(density, c.P1, c.P2, c.Radius, out meshCenterOfMass); } break; } parts.Add(meshPart); materials.Add(new Material(elasticity, roughness)); Vector3.Multiply(ref meshCenterOfMass, meshMass.Mass, out meshCenterOfMass); Vector3.Add(ref centerOfMass, ref meshCenterOfMass, out centerOfMass); mass.Mass += meshMass.Mass; meshMass.Inertia.M44 = 0f; Matrix.Add(ref mass.Inertia, ref meshMass.Inertia, out mass.Inertia); } // compute mass properties Vector3.Divide(ref centerOfMass, mass.Mass, out centerOfMass); mass.Inertia.M44 = 1f; MassProperties.TranslateInertiaTensor(ref mass.Inertia, -mass.Mass, centerOfMass, out mass.Inertia); if (centerOfMass.Length() >= Constants.Epsilon) { var transform = Matrix.CreateTranslation(-centerOfMass.X, -centerOfMass.Y, -centerOfMass.Z); foreach (var p in parts) { p.Transform(ref transform); } transform = model.Root.Transform; transform.M41 -= centerOfMass.X; transform.M42 -= centerOfMass.Y; transform.M43 -= centerOfMass.Z; model.Root.Transform = transform; } mass = new MassProperties(mass.Mass, mass.Inertia); var rbm = new RigidBodyModel(mass, parts.ToArray(), materials.ToArray()); // remove non-visual nodes if (nodesToRemove.Length > 0) { foreach (var node in nodesToRemove) input.Children.Remove(node); model = base.Process(input, context); } model.Tag = rbm; return model; }
internal static void EffectiveMassMatrix(ref MassProperties a, ref MassProperties b, ref Vector3 offsetA, ref Vector3 offsetB, out Matrix k) { Matrix ka = new Matrix(), kb = new Matrix(); if(a.Mass < float.PositiveInfinity) a.InverseMassMatrix(ref offsetA, out ka); if(b.Mass < float.PositiveInfinity) b.InverseMassMatrix(ref offsetB, out kb); Matrix.Add(ref ka, ref kb, out k); k.M44 = 1f; Matrix.Invert(ref k, out k); }
internal static float EffectiveMass(ref MassProperties a, ref MassProperties b, ref Vector3 offsetA, ref Vector3 offsetB, ref Vector3 normal) { Vector3 v; float normalMass = 0f, f; if (a.Mass < float.PositiveInfinity) { normalMass = a.MassInverse; Vector3.Cross(ref offsetA, ref normal, out v); Vector3.Transform(ref v, ref a.InertiaInverse, out v); Vector3.Cross(ref v, ref offsetA, out v); Vector3.Dot(ref normal, ref v, out f); normalMass += f; } if (b.Mass < float.PositiveInfinity) { normalMass += b.MassInverse; Vector3.Cross(ref offsetB, ref normal, out v); Vector3.Transform(ref v, ref b.InertiaInverse, out v); Vector3.Cross(ref v, ref offsetB, out v); Vector3.Dot(ref normal, ref v, out f); normalMass += f; } if (normalMass < Constants.Epsilon) normalMass = Constants.Epsilon; return 1f / normalMass; }
/// <summary> /// Solve the constraint for position. /// </summary> /// <returns>Returns a value indicating whether the constraint has been satisfied.</returns> public override bool ProcessPosition() { RigidBody a = BodyA, b = BodyB; if (_state == LimitState.Between) { return(true); } // recalculate vitals Vector3.Transform(ref _bodyPointA, ref a.World.Combined, out _worldOffsetA); Vector3.Transform(ref _bodyPointB, ref b.World.Combined, out _worldOffsetB); Vector3.Subtract(ref _worldOffsetA, ref _worldOffsetB, out _normal); Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA); Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB); _distance = _normal.Length(); Vector3.Divide(ref _normal, _distance, out _normal); _mass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, ref _normal); // the error depends on the current limit state float error = 0f; switch (_state) { case LimitState.Equal: if (_distance > _maxDistance) { error = _distance - _maxDistance; } else if (_distance < _minDistance) { error = _distance - _minDistance; } break; case LimitState.Min: error = MathHelper.Min(_distance - _minDistance, 0f); break; case LimitState.Max: error = MathHelper.Max(_distance - _maxDistance, 0f); break; } if (Math.Abs(error) <= this.Manager.LinearErrorTolerance) { return(true); } // clamp impulse Vector3 impulse, oldImpulse = _pImpulse; Vector3.Multiply(ref _normal, error * _mass * this.Manager.PositionCorrectionFactor, out impulse); Vector3.Add(ref _pImpulse, ref impulse, out _pImpulse); float d; Vector3.Dot(ref _normal, ref _pImpulse, out d); if (_state == LimitState.Min && d > 0f || _state == LimitState.Max && d < 0f) { _pImpulse = Vector3.Zero; } Vector3.Subtract(ref _pImpulse, ref oldImpulse, out impulse); // apply impulse b.ApplyFlashImpulse(ref impulse, ref _worldOffsetB); Vector3.Negate(ref impulse, out impulse); a.ApplyFlashImpulse(ref impulse, ref _worldOffsetA); return(false); }
public void CreateScene(int sceneNumber) { _physics.Clear(); _markers.Clear(); Room room = new Room(this); _physics.Add(room); _physics.Gravity = new Vector3(0f, 0f, -9.8f); Model cubeModel = this.Content.Load<Model>("models/small_cube"); Model obeliskModel = this.Content.Load<Model>("models/obelisk"); Model sphereModel = this.Content.Load<Model>("models/sphere"); Model capsuleModel = this.Content.Load<Model>("models/capsule"); Model torusModel = this.Content.Load<Model>("models/torus"); Model slabModel = this.Content.Load<Model>("models/slab"); Model triangleModel = this.Content.Load<Model>("models/triangle"); switch (sceneNumber) { case 1: { for (int i = 0; i < 12; i++) { var cube = new SolidThing(this, cubeModel); cube.SetWorld(new Vector3(0f, 0f, 0.25f + 0.51f * i)); _physics.Add(cube); } } break; case 2: { for (int i = 0; i < 7; i++) { for (int j = 0; j < 7 - i; j++) { var cube = new SolidThing(this, cubeModel); cube.SetWorld(new Vector3(0f, 0.501f * j + 0.25f * i, 0.5f + 0.55f * i)); _physics.Add(cube); } } } break; case 3: { for (int i = 0; i < 6; i++) { for (int j = 0; j < 6 - i; j++) { var cube = new SolidThing(this, cubeModel); cube.SetWorld(new Vector3(0f, 2.2f * j + 1f * i - 4.1f, 0.75f * i + 0.25f)); _physics.Add(cube); } } for (int i = 0; i < 6; i++) { for (int j = 0; j < 5 - i; j++) { var plank = new SolidThing(this, obeliskModel); plank.SetWorld(new Vector3(0f, 2.2f * j + 1f * i + 1f - 4f, 0.75f * i + 0.65f), Quaternion.CreateFromAxisAngle(Vector3.UnitZ, MathHelper.ToRadians(90f))); _physics.Add(plank); } } } break; case 4: { int size = 9; #if WINDOWS #else size = 4; #endif for (int i = 0; i < size; i++) { for (int j = 0; j < size - i; j++) { for (int k = 0; k < size - i; k++) { var sphere = new SolidThing(this, sphereModel); sphere.SetWorld(new Vector3( 0.501f * j + 0.25f * i, 0.501f * k + 0.25f * i, 0.501f * i + 0.5f ), Quaternion.Identity); _physics.Add(sphere); } } } } break; case 5: { var plank = new SolidThing(this, obeliskModel); plank.SetWorld(new Vector3(0.0f, 0.0f, 4.0f), Quaternion.CreateFromAxisAngle(Vector3.UnitY, MathHelper.ToRadians(15f))); MassProperties immovableMassProperties = new MassProperties(float.PositiveInfinity, Matrix.Identity); plank.MassProperties = immovableMassProperties; _physics.Add(plank); var sphere = new SolidThing(this, sphereModel); sphere.SetWorld(new Vector3(-4.9f, 0.0f, 9.0f), Quaternion.Identity); _physics.Add(sphere); // int size = 9; //#if WINDOWS //#else // size = 4; //#endif // var models = new Model[] { cubeModel, sphereModel }; // for (int i = 0; i < size; i++) // { // for (int j = 0; j < size - i; j++) // { // for (int k = 0; k < size - i; k++) // { // var sphere = new SolidThing(this, i % 2 == 0 ? sphereModel : cubeModel); // sphere.SetWorld(new Vector3( // 0.501f * j + 0.25f * i, // 0.501f * k + 0.25f * i, // 0.501f * i + 0.5f // ), Quaternion.Identity); // _physics.Add(sphere); // } // } // } } break; case 6: { int size = 9; #if WINDOWS #else size = 4; #endif var models = new Model[] { cubeModel, sphereModel, capsuleModel }; for (int i = 0; i < size; i++) { for (int j = 0; j < size - i; j++) { for (int k = 0; k < size - i; k++) { var sphere = new SolidThing(this, models[_rand.Next(3)]); sphere.SetWorld(new Vector3( 0.501f * j + 0.25f * i, 0.501f * k + 0.25f * i, 1f * i + 0.5f)); _physics.Add(sphere); } } } } break; case 7: { var o = new SolidThing(this, torusModel); o.SetWorld(new Vector3(0f, 0f, 0.5f), Quaternion.CreateFromAxisAngle(Vector3.UnitY, -MathHelper.PiOver2)); _physics.Add(o); o = new SolidThing(this, torusModel); o.SetWorld(new Vector3(0f, 0f, 4f), Quaternion.CreateFromAxisAngle(Vector3.UnitY, -MathHelper.PiOver4)); _physics.Add(o); o = new SolidThing(this, slabModel); o.SetWorld(new Vector3(-4f, 4f, 2f)); _physics.Add(o); o = new SolidThing(this, this.Content.Load<Model>("models/cone")); o.SetWorld(new Vector3(-4f, -4f, 1f), Quaternion.CreateFromAxisAngle(Vector3.UnitZ, MathHelper.PiOver2)); _physics.Add(o); o = new SolidThing(this, cubeModel); o.SetWorld(new Vector3(-4f, 6.1f, 3f)); _physics.Add(o); } break; case 8: { RigidBody oLast = null; for (int i = 0; i < 10; i++) { var o = new SolidThing(this, capsuleModel); o.SetWorld(new Vector3(0f, 0f, 9.5f - i)); _physics.Add(o); if (i == 0) { var j = new PointConstraint(o, room, new Vector3(0f, 0f, 10f)); j.IsCollisionEnabled = false; _physics.Add(j); } else { var j = new PointConstraint(oLast, o, new Vector3(0f, 0f, 10f - (float)i)); j.IsCollisionEnabled = false; _physics.Add(j); } oLast = o; } var a = new SolidThing(this, cubeModel); a.SetWorld(new Vector3(1f, 0f, 0.25f)); _physics.Add(a); var b = new SolidThing(this, cubeModel); b.SetWorld(new Vector3(1f, 0f, 0.75f)); _physics.Add(b); var j2 = new RevoluteJoint(b, a, new Vector3(1.25f, 0f, 0.5f), Vector3.UnitY, 0f, MathHelper.PiOver2); j2.IsCollisionEnabled = false; _physics.Add(j2); a = new SolidThing(this, cubeModel); a.SetWorld(new Vector3(1f, 1f, 0.25f)); _physics.Add(a); b = new SolidThing(this, cubeModel); b.SetWorld(new Vector3(1f, 1f, 0.75f)); _physics.Add(b); var j4 = new GenericConstraint(b, a, new Frame(new Vector3(1f, 1f, 0.5f)), Axes.All, Vector3.Zero, new Vector3(0f, 0f, 0.5f), Axes.All, Vector3.Zero, Vector3.Zero); j4.IsCollisionEnabled = false; _physics.Add(j4); a = new SolidThing(this, cubeModel); a.SetWorld(new Vector3(1f, 2f, 0.25f)); _physics.Add(a); b = new SolidThing(this, cubeModel); b.SetWorld(new Vector3(1f, 2f, 0.75f)); _physics.Add(b); var j5 = new GenericConstraint(b, a, new Frame(new Vector3(1f, 2f, 0.5f)), Axes.All, new Vector3(-0.125f, -0.125f, 0f), new Vector3(0.125f, 0.125f, 0f), Axes.All, Vector3.Zero, Vector3.Zero); j5.IsCollisionEnabled = false; _physics.Add(j5); a = new SolidThing(this, sphereModel); a.SetWorld(new Vector3(2f, 0f, 2f)); _physics.Add(a); b = new SolidThing(this, sphereModel); b.SetWorld(new Vector3(2f, 0f, 1f)); _physics.Add(b); var g1 = new SpringForce(a, b, Vector3.Zero, Vector3.Zero, 1f, 5f, 0.05f); _physics.Add(g1); var j3 = new WorldPointConstraint(a, new Vector3(2f, 0f, 2f)); _physics.Add(j3); } break; case 9: { var a = new SolidThing(this, sphereModel); a.Skin.Remove(a.Skin[0]); a.Skin.Add(new SpherePart(new Sphere(Vector3.Zero, 0.25f)), new Material(0f, 0.5f)); a.SetWorld(7.0f, new Vector3(0f, 0f, 5f), Quaternion.Identity); a.MassProperties = MassProperties.Immovable; _physics.Add(a); _physics.Add(new SingularityForce(new Vector3(0f, 0f, 5f), 1E12f)); _physics.Gravity = Vector3.Zero; var b = new SolidThing(this, cubeModel); b.SetWorld(new Vector3(0f, 0f, 8f), Quaternion.CreateFromAxisAngle(Vector3.UnitX, MathHelper.PiOver4 / 2.0f) * Quaternion.CreateFromAxisAngle(Vector3.UnitY, MathHelper.PiOver4) ); _physics.Add(b); } break; default: break; } }
/// <summary> /// Prepare the contact constraint for iterative processing within a single frame. Computes the desired target velocities /// to attempt to prevent inter-penetration. /// </summary> public override void PreProcess() { if (this.IsCollisionSuppressed) { return; } RigidBody a = BodyA, b = BodyB; var cached = b.Manager.ContactCache.Get(a, b); bool isWarmStarted = cached != null && cached.Count == _count; // calculate relative force applied during this frame Vector3 va = Vector3.Zero, vb = Vector3.Zero; float forceMag; if (a.IsMovable) { Vector3.Multiply(ref a.Force, this.Manager.TimeStep * a.Mass.MassInverse, out va); } if (b.IsMovable) { Vector3.Multiply(ref b.Force, this.Manager.TimeStep * b.Mass.MassInverse, out vb); } Vector3.Add(ref va, ref vb, out va); Vector3.Dot(ref _normal, ref va, out forceMag); forceMag = MathHelper.Min(forceMag, 0f); for (int i = 0; i < _count; i++) { var p = _points[i]; // calculate movement along normal and tangent float normalDelta; a.GetVelocityAtPoint(ref p.OffsetA, out va); b.GetVelocityAtPoint(ref p.OffsetB, out vb); Vector3.Subtract(ref va, ref vb, out va); Vector3.Dot(ref _normal, ref va, out normalDelta); float tangentDelta; Vector3.Multiply(ref _normal, normalDelta, out p.Tangent); Vector3.Subtract(ref va, ref p.Tangent, out p.Tangent); Vector3.Dot(ref p.Tangent, ref va, out tangentDelta); if (p.Tangent.LengthSquared() >= Constants.Epsilon) { p.Tangent.Normalize(); Vector3.Negate(ref p.Tangent, out p.Tangent); } else { p.Tangent = Vector3.Zero; } // calculate effective mass along tangent and normal p.NormalMass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref p.OffsetA, ref p.OffsetB, ref _normal); p.TangentMass = p.Tangent != Vector3.Zero ? MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref p.OffsetA, ref p.OffsetB, ref p.Tangent) : 0f; // calculate target velocity float restitution = Math.Max(_restitution * -(normalDelta - forceMag), 0f); float penetration = (p.Depth - this.Manager.LinearErrorTolerance); if (restitution < this.Manager.MinRestitution) { if (penetration > 0f) { p.Target = penetration * this.Manager.PenetrationBias; } else { float scale = MathHelper.Clamp(-0.1f * penetration / this.Manager.LinearErrorTolerance, Constants.Epsilon, 1f); p.Target = scale * (p.Depth - this.Manager.LinearErrorTolerance) * this.Manager.TimeStepInverse; } } else { p.Target = Math.Max(Math.Max(penetration * this.Manager.PenetrationBias, 0f), restitution); } p.Impulse = 0f; p.TangentImpulse = 0f; p.PositionImpulse = 0f; if (isWarmStarted) { Vector3 impulse, tangentImpulse; // find the best cached point var bestPoint = new CachedContactPoint(); float bestDistance = float.MaxValue; for (int j = 0; j < cached.Points.Length; j++) { float d1, d2; Vector3.DistanceSquared(ref cached.Points[j].OffsetA, ref p.OffsetA, out d1); Vector3.DistanceSquared(ref cached.Points[j].OffsetB, ref p.OffsetB, out d2); if (d1 + d2 < bestDistance) { bestDistance = d1 + d2; bestPoint = cached.Points[j]; } } p.Impulse = bestPoint.NormalImpulse; float tangentImpulseMag = MathHelper.Clamp(-tangentDelta * p.TangentMass, 0f, _friction * p.Impulse); p.TangentImpulse = tangentImpulseMag * p.NormalMass; if (Math.Abs(p.Impulse) >= Constants.Epsilon) { Vector3.Multiply(ref _normal, p.Impulse, out impulse); Vector3.Multiply(ref p.Tangent, p.TangentImpulse, out tangentImpulse); Vector3.Add(ref impulse, ref tangentImpulse, out impulse); a.ApplyImpulse(ref impulse, ref p.OffsetA); Vector3.Negate(ref impulse, out impulse); b.ApplyImpulse(ref impulse, ref p.OffsetB); } } _points[i] = p; } // calculate an averaged contact point for help with stabilization during position correction if (_count > 2 && _count < _points.Length) { var ap = _points[_count]; ap.Depth = float.MaxValue; for (int i = 0; i < _count; i++) { var p = _points[i]; float depth; Vector3 pa, pb; Vector3.Add(ref a.World.Position, ref p.OffsetA, out pa); Vector3.Add(ref b.World.Position, ref p.OffsetB, out pb); Vector3.Add(ref ap.OffsetA, ref pa, out ap.OffsetA); Vector3.Add(ref ap.OffsetB, ref pb, out ap.OffsetB); Vector3.Subtract(ref pb, ref pa, out pa); Vector3.Dot(ref _normal, ref pa, out depth); if (depth < ap.Depth) { ap.Depth = depth; } } Vector3.Divide(ref ap.OffsetA, _count, out ap.OffsetA); Vector3.Divide(ref ap.OffsetB, _count, out ap.OffsetB); ap.NormalMass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref ap.OffsetA, ref ap.OffsetB, ref _normal); ap.PositionImpulse = 0f; _points[_count] = ap; } }