private void PickRigidBody(RigidBody body, ref Vector3 pickPosition) { if (body.IsStaticObject || body.IsKinematicObject) { return; } _pickedBody = body; _pickedBody.ActivationState = ActivationState.DisableDeactivation; DiscreteDynamicsWorld world = _demo.Simulation.World; Vector3 localPivot = Vector3.TransformCoordinate(pickPosition, Matrix.Invert(body.CenterOfMassTransform)); if (_demo.Input.KeysDown.Contains(Keys.ShiftKey)) { var dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false) { LinearLowerLimit = Vector3.Zero, LinearUpperLimit = Vector3.Zero, AngularLowerLimit = Vector3.Zero, AngularUpperLimit = Vector3.Zero }; world.AddConstraint(dof6); _rigidBodyPickConstraint = dof6; dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5); } else { var p2p = new Point2PointConstraint(body, localPivot); world.AddConstraint(p2p); _rigidBodyPickConstraint = p2p; p2p.Setting.ImpulseClamp = 30; //very weak constraint for picking p2p.Setting.Tau = 0.001f; /* * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 0); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 1); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 2); */ } }
public void moveObject(Vector3 rayFrom, Vector3 rayTo) { if (_pick_constraint != null) { Vector3 dir = rayTo - rayFrom; dir.Normalize(); dir *= _picking_distance_current; if (_pick_constraint.ConstraintType == TypedConstraintType.D6) { Generic6DofConstraint pickCon = _pick_constraint as Generic6DofConstraint; //keep it at the same picking distance Matrix tempFrameOffsetA = pickCon.FrameOffsetA; tempFrameOffsetA.Origin = rayFrom + dir; pickCon.SetFrames(tempFrameOffsetA, pickCon.FrameOffsetB); } else { Point2PointConstraint pickCon = _pick_constraint as Point2PointConstraint; //keep it at the same picking distance pickCon.PivotInB = rayFrom + dir; } } }
//called by Physics World just before constraint is added to world. //the current constraint properties are used to rebuild the constraint. internal override bool _BuildConstraint() { BPhysicsWorld world = BPhysicsWorld.Get(); if (constraintPtr != null) { if (isInWorld && world != null) { isInWorld = false; world.RemoveConstraint(constraintPtr); } } if (targetRigidBodyA == null) { Debug.LogError("Constraint target rigid body was not set."); return(false); } RigidBody rba = (RigidBody)targetRigidBodyA.GetCollisionObject(); if (rba == null) { Debug.LogError("Constraint could not get bullet RigidBody from target rigid body"); return(false); } if (constraintType == ConstraintType.constrainToAnotherBody) { RigidBody rbb = (RigidBody)targetRigidBodyB.GetCollisionObject(); if (rbb == null) { Debug.LogError("Constraint could not get bullet RigidBody from target rigid body"); return(false); } BulletSharp.Math.Matrix frameInA = BulletSharp.Math.Matrix.AffineTransformation(1f, Quaternion.LookRotation(localForwardInA, localUpInA).ToBullet(), localPointInA.ToBullet()); BulletSharp.Math.Matrix frameInB = BulletSharp.Math.Matrix.AffineTransformation(1f, Quaternion.LookRotation(localForwardInB, localUpInB).ToBullet(), localPointInB.ToBullet()); constraintPtr = new Generic6DofConstraint(rba, rbb, frameInA, frameInB, false); } else { BulletSharp.Math.Matrix frameInA = BulletSharp.Math.Matrix.AffineTransformation(1f, Quaternion.LookRotation(localForwardInA, localUpInA).ToBullet(), localPointInA.ToBullet()); constraintPtr = new Generic6DofConstraint(rba, frameInA, false); } constraintPtr.Userobject = this; Generic6DofConstraint sl = (Generic6DofConstraint)constraintPtr; sl.LinearLowerLimit = linearLimitLower.ToBullet(); sl.LinearUpperLimit = linearLimitUpper.ToBullet(); sl.AngularLowerLimit = angularLimitLower.ToBullet(); sl.AngularUpperLimit = angularLimitUpper.ToBullet(); sl.TranslationalLimitMotor.TargetVelocity = motorLinearTargetVelocity.ToBullet(); sl.TranslationalLimitMotor.MaxMotorForce = motorLinearMaxMotorForce.ToBullet(); return(true); }
private void CreateGeneric6DofConstraint() { const float mass = 1.0f; RigidBody fixedBody = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), cubeShape); fixedBody.ActivationState = ActivationState.DisableDeactivation; RigidBody dynamicbody = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), cubeShape); dynamicbody.ActivationState = ActivationState.DisableDeactivation; Matrix frameInA = Matrix.Translation(-5, 0, 0); Matrix frameInB = Matrix.Translation(5, 0, 0); bool useLinearReferenceFrameA = true; var generic6Dof = new Generic6DofConstraint(fixedBody, dynamicbody, frameInA, frameInB, useLinearReferenceFrameA) { LinearLowerLimit = new Vector3(-10, -2, -1), LinearUpperLimit = new Vector3(10, 2, 1), //LinearLowerLimit = new Vector3(-10, 0, 0), //LinearUpperLimit = new Vector3(10, 0, 0), //LinearLowerLimit = new Vector3(0, 0, 0), //LinearUpperLimit = new Vector3(0, 0, 0), AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f), AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f), //AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0), //AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0), //AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI), //AngularUpperLimit = new Vector3(0, 0, (float)Math.PI), //AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f), //AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f), //AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f), //AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f), //AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f), //AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f), //AngularLowerLimit = new Vector3(-1, 0, 0), //AngularUpperLimit = new Vector3(1, 0, 0), DebugDrawSize = 5.0f }; //generic6Dof.TranslationalLimitMotor.EnableMotor[0] = true; //generic6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0); //generic6Dof.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0); World.AddConstraint(generic6Dof, true); }
//GenericoDofConstraint public IGeneric6DofConstraintImp AddGeneric6DofConstraint(IRigidBodyImp rigidBodyA, float4x4 frameInA, bool useReferenceFrameA) { var rigidBodyAImp = (RigidBodyImp)rigidBodyA; var btRigidBodyA = rigidBodyAImp._rbi; var btFframeInA = Translater.Float4X4ToBtMatrix(frameInA); var btGeneric6DofConstraint = new Generic6DofConstraint(btRigidBodyA, btFframeInA, useReferenceFrameA); BtWorld.AddConstraint(btGeneric6DofConstraint); var retval = new Generic6DofConstraintImp(); retval._g6dofci = btGeneric6DofConstraint; btGeneric6DofConstraint.UserObject = retval; return(retval); }
void CreateParticles() { //ZeroBody zb = new ZeroShape(); BoxShape brick = new BoxShape(0.1, 0.1, 0.1); RigidBody fix = CreateBody(0, brick, new Vector3(0, 0, 0)); fix.SetSleepingThresholds(0, 0); fix.SetAnisotropicFriction(Vector3.Zero, AnisotropicFrictionFlags.FrictionDisabled); fix.Friction = 0; fix.SetDamping(0, 0); Random rand = new Random(); SphereShape ss = new SphereShape(particleSize); for (int xx = 0; xx < 115; xx++) { for (int yy = 0; yy < 90; yy++) { RigidBody r = CreateBody(1, ss, new Vector3(5 + xx * 0.51, 5 + yy * 0.51, 0)); r.Friction = 0.0; r.Restitution = 1; r.RollingFriction = 0; r.SetDamping(0.01, 0.1); Particles.Add(r); r.SetSleepingThresholds(0, 0); r.SetAnisotropicFriction(Vector3.Zero, AnisotropicFrictionFlags.FrictionDisabled); var c = new Generic6DofConstraint(fix, r, Matrix.Translation(1, 0, 0), Matrix.Translation(1, 0, 0), true); //c.BreakingImpulseThreshold = 999999999999999; c.SetLimit(0, 0, 150); c.SetLimit(1, 0, 100); c.SetLimit(2, 0, 0); c.SetLimit(3, 0, 0); c.SetLimit(4, 0, 0); c.SetLimit(5, 0, 0); //r.ApplyCentralImpulse(new Vector3(10, 10, 0)); //r.ApplyCentralForce(new Vector3(5, 5, 0)); world.AddConstraint(c); } } Console.WriteLine("Created"); }
// Create a slider using the generic D6 constraint private void CreateD6Slider() { const float mass = 1.0f; Vector3 sliderAxis = Vector3.UnitX; const float angle = (float)Math.PI / 4; Matrix trans = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(0, 10, 0); RigidBody body = LocalCreateRigidBody(mass, trans, cubeShape); body.ActivationState = ActivationState.DisableDeactivation; RigidBody fixedBody = LocalCreateRigidBody(0, trans, null); World.AddRigidBody(fixedBody); Matrix frameInA = Matrix.Translation(0, 5, 0); Matrix frameInB = Matrix.Translation(0, 5, 0); Vector3 lowerSliderLimit = new Vector3(-10, 0, 0); Vector3 hiSliderLimit = new Vector3(10, 0, 0); //const bool useLinearReferenceFrameA = false; //use fixed frame B for linear llimits const bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits var slider = new Generic6DofConstraint(fixedBody, body, frameInA, frameInB, useLinearReferenceFrameA) { LinearLowerLimit = lowerSliderLimit, LinearUpperLimit = hiSliderLimit, //range should be small, otherwise singularities will 'explode' the constraint //AngularLowerLimit = new Vector3(-1.5f,0,0), //AngularUpperLimit = new Vector3(1.5f,0,0), //AngularLowerLimit = new Vector3(0,0,0), //AngularUpperLimit = new Vector3(0,0,0), AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0), AngularUpperLimit = new Vector3(1.5f, 0, 0), DebugDrawSize = 5 }; //slider.TranslationalLimitMotor.EnableMotor[0] = true; slider.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0); slider.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0); World.AddConstraint(slider); }
void MovePickedBody() { if (pickConstraint != null) { Vector3 rayFrom = Freelook.Eye; Vector3 newRayTo = GetRayTo(Input.MousePoint, rayFrom, Freelook.Target, Graphics.FieldOfView); //keep it at the same picking distance Vector3 dir = newRayTo - rayFrom; dir.Normalize(); dir *= oldPickingDist; if (pickConstraint.ConstraintType == TypedConstraintType.D6) { Generic6DofConstraint pickCon = pickConstraint as Generic6DofConstraint; //keep it at the same picking distance Matrix tempFrameOffsetA = pickCon.FrameOffsetA; tempFrameOffsetA.Origin = rayFrom + dir; pickCon.FrameOffsetA = tempFrameOffsetA; } else { Point2PointConstraint pickCon = pickConstraint as Point2PointConstraint; //keep it at the same picking distance pickCon.PivotInB = rayFrom + dir; } } else if (pickingMultiBodyPoint2Point != null) { Vector3 rayFrom = Freelook.Eye; Vector3 newRayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView); Vector3 dir = (newRayTo - rayFrom); dir.Normalize(); dir *= oldPickingDist; pickingMultiBodyPoint2Point.PivotInB = rayFrom + dir; } }
public virtual void OnHandleInput() { if (Input.KeysPressed.Count != 0) { switch (Input.KeysPressed[0]) { case Keys.Escape: case Keys.Q: Graphics.Form.Close(); return; case Keys.F3: IsDebugDrawEnabled = !IsDebugDrawEnabled; break; case Keys.F8: Input.ClearKeyCache(); GraphicsLibraryManager.ExitWithReload = true; Graphics.Form.Close(); break; case Keys.F11: Graphics.IsFullScreen = !Graphics.IsFullScreen; break; case (Keys.Control | Keys.F): const int maxSerializeBufferSize = 1024 * 1024 * 5; DefaultSerializer serializer = new DefaultSerializer(maxSerializeBufferSize); World.Serialize(serializer); byte[] dataBytes = new byte[serializer.CurrentBufferSize]; Marshal.Copy(serializer.BufferPointer, dataBytes, 0, dataBytes.Length); System.IO.FileStream file = new System.IO.FileStream("world.bullet", System.IO.FileMode.Create); file.Write(dataBytes, 0, dataBytes.Length); file.Dispose(); break; case Keys.G: //shadowsEnabled = !shadowsEnabled; break; case Keys.Space: ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView)); break; case Keys.Return: ClientResetScene(); break; } } if (Input.MousePressed != MouseButtons.None) { Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView); if (Input.MousePressed == MouseButtons.Right) { if (_world != null) { Vector3 rayFrom = Freelook.Eye; ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo); _world.RayTestRef(ref rayFrom, ref rayTo, rayCallback); if (rayCallback.HasHit) { RigidBody body = rayCallback.CollisionObject as RigidBody; if (body != null) { if (!(body.IsStaticObject || body.IsKinematicObject)) { pickedBody = body; pickedBody.ActivationState = ActivationState.DisableDeactivation; Vector3 pickPos = rayCallback.HitPointWorld; Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform)); if (Input.KeysDown.Contains(Keys.ShiftKey)) { Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false) { LinearLowerLimit = Vector3.Zero, LinearUpperLimit = Vector3.Zero, AngularLowerLimit = Vector3.Zero, AngularUpperLimit = Vector3.Zero }; _world.AddConstraint(dof6); pickConstraint = dof6; dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5); } else { Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot); _world.AddConstraint(p2p); pickConstraint = p2p; p2p.Setting.ImpulseClamp = 30; //very weak constraint for picking p2p.Setting.Tau = 0.001f; /* * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 0); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 1); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 2); */ } oldPickingDist = (pickPos - rayFrom).Length; } } } rayCallback.Dispose(); } } } else if (Input.MouseReleased == MouseButtons.Right) { RemovePickingConstraint(); } // Mouse movement if (Input.MouseDown == MouseButtons.Right) { if (pickConstraint != null) { Vector3 newRayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView); if (pickConstraint.ConstraintType == TypedConstraintType.D6) { Generic6DofConstraint pickCon = pickConstraint as Generic6DofConstraint; //keep it at the same picking distance Vector3 rayFrom = Freelook.Eye; Vector3 dir = newRayTo - rayFrom; dir.Normalize(); dir *= oldPickingDist; Vector3 newPivotB = rayFrom + dir; Matrix tempFrameOffsetA = pickCon.FrameOffsetA; tempFrameOffsetA.M41 = newPivotB.X; tempFrameOffsetA.M42 = newPivotB.Y; tempFrameOffsetA.M43 = newPivotB.Z; pickCon.SetFrames(tempFrameOffsetA, pickCon.FrameOffsetB); } else { Point2PointConstraint pickCon = pickConstraint as Point2PointConstraint; //keep it at the same picking distance Vector3 rayFrom = Freelook.Eye; Vector3 dir = newRayTo - rayFrom; dir.Normalize(); dir *= oldPickingDist; pickCon.PivotInB = rayFrom + dir; } } } }
protected virtual void OnHandleInput() { if (Input.KeysPressed.Count != 0) { switch (Input.KeysPressed[0]) { case Keys.Escape: case Keys.Q: Form.Close(); return; case Keys.F1: MessageBox.Show( "Move using mouse and WASD + shift\n" + "Space - Shoot box\n" + "Q - Quit\n\n" + "G - Toggle shadows\n" + "L - Toggle deferred lighting\n", "Help"); return; case Keys.F3: //IsDebugDrawEnabled = !IsDebugDrawEnabled; break; case Keys.F11: //ToggleFullScreen(); break; case (Keys.Control | Keys.F): const int maxSerializeBufferSize = 1024 * 1024 * 5; using (var serializer = new DefaultSerializer(maxSerializeBufferSize)) { PhysicsContext.World.Serialize(serializer); byte[] dataBytes = new byte[serializer.CurrentBufferSize]; Marshal.Copy(serializer.BufferPointer, dataBytes, 0, dataBytes.Length); using (var file = new System.IO.FileStream("world.bullet", System.IO.FileMode.Create)) { file.Write(dataBytes, 0, dataBytes.Length); } } break; case Keys.G: shadowsEnabled = !shadowsEnabled; break; case Keys.L: deferredLightingEnabled = !deferredLightingEnabled; break; case Keys.Space: PhysicsContext.ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView)); break; } } if (Input.MousePressed != MouseButtons.None) { Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView); if (Input.MousePressed == MouseButtons.Right) { if (PhysicsContext.World != null) { Vector3 rayFrom = Freelook.Eye; var rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo); PhysicsContext.World.RayTest(rayFrom, rayTo, rayCallback); if (rayCallback.HasHit) { RigidBody body = rayCallback.CollisionObject as RigidBody; if (body != null) { if (!(body.IsStaticObject || body.IsKinematicObject)) { pickedBody = body; pickedBody.ActivationState = ActivationState.DisableDeactivation; Vector3 pickPos = rayCallback.HitPointWorld; Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform)); if (Input.KeysDown.Contains(Keys.ShiftKey)) { Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false) { LinearLowerLimit = Vector3.Zero, LinearUpperLimit = Vector3.Zero, AngularLowerLimit = Vector3.Zero, AngularUpperLimit = Vector3.Zero }; pickConstraint = dof6; dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5); } else { var p2p = new Point2PointConstraint(body, localPivot); pickConstraint = p2p; p2p.Setting.ImpulseClamp = 30; //very weak constraint for picking p2p.Setting.Tau = 0.001f; /* * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 0); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 1); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 2); */ } PhysicsContext.World.AddConstraint(pickConstraint); oldPickingDist = (pickPos - rayFrom).Length(); } } } rayCallback.Dispose(); } } } else if (Input.MouseReleased == MouseButtons.Right) { RemovePickingConstraint(); } // Mouse movement if (Input.MouseDown == MouseButtons.Right) { if (pickConstraint != null) { Vector3 newRayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView); if (pickConstraint.ConstraintType == TypedConstraintType.D6) { Generic6DofConstraint pickCon = pickConstraint as Generic6DofConstraint; //keep it at the same picking distance Vector3 rayFrom = Freelook.Eye; Vector3 dir = newRayTo - rayFrom; dir.Normalize(); dir *= oldPickingDist; Vector3 newPivotB = rayFrom + dir; Matrix tempFrameOffsetA = pickCon.FrameOffsetA; tempFrameOffsetA.M41 = newPivotB.X; tempFrameOffsetA.M42 = newPivotB.Y; tempFrameOffsetA.M43 = newPivotB.Z; pickCon.SetFrames(tempFrameOffsetA, pickCon.FrameOffsetB); } else { Point2PointConstraint pickCon = pickConstraint as Point2PointConstraint; //keep it at the same picking distance Vector3 rayFrom = Freelook.Eye; Vector3 dir = newRayTo - rayFrom; dir.Normalize(); dir *= oldPickingDist; pickCon.PivotInB = rayFrom + dir; } } } }
public Physics() { SetupEmptyDynamicsWorld(); CollisionShape groundShape = new BoxShape(50, 1, 50); //CollisionShape groundShape = new StaticPlaneShape(Vector3.UnitY, 40); CollisionShapes.Add(groundShape); RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape); body.UserObject = "Ground"; CollisionShape shape = new BoxShape(new Vector3(CubeHalfExtents)); CollisionShapes.Add(shape); float mass = 1.0f; RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), shape); RigidBody body1 = null;//LocalCreateRigidBody(mass, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), shape); //RigidBody body1 = LocalCreateRigidBody(0, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), null); //body1.ActivationState = ActivationState.DisableDeactivation; //body1.SetDamping(0.3f, 0.3f); Vector3 pivotInA = new Vector3(CubeHalfExtents, -CubeHalfExtents, -CubeHalfExtents); Vector3 axisInA = new Vector3(0, 0, 1); Vector3 pivotInB; if (body1 != null) { Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform; pivotInB = Vector3.TransformCoordinate(pivotInA, transform); } else { pivotInB = pivotInA; } Vector3 axisInB; if (body1 != null) { Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform; axisInB = Vector3.TransformCoordinate(axisInA, transform); } else { axisInB = Vector3.TransformCoordinate(axisInA, body0.CenterOfMassTransform); } if (P2P) { TypedConstraint p2p = new Point2PointConstraint(body0, pivotInA); //TypedConstraint p2p = new Point2PointConstraint(body0,body1,pivotInA,pivotInB); //TypedConstraint hinge = new HingeConstraint(body0,body1,pivotInA,pivotInB,axisInA,axisInB); World.AddConstraint(p2p); p2p.DebugDrawSize = 5; } else { HingeConstraint hinge = new HingeConstraint(body0, pivotInA, axisInA); //use zero targetVelocity and a small maxMotorImpulse to simulate joint friction //float targetVelocity = 0.f; //float maxMotorImpulse = 0.01; float targetVelocity = 1.0f; float maxMotorImpulse = 1.0f; hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse); World.AddConstraint(hinge); hinge.DebugDrawSize = 5; } //create a slider, using the generic D6 constraint Vector3 sliderWorldPos = new Vector3(0, 10, 0); Vector3 sliderAxis = Vector3.UnitX; float angle = 0;//SIMD_RADS_PER_DEG * 10.f; Matrix trans = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(sliderWorldPos); d6body0 = LocalCreateRigidBody(mass, trans, shape); d6body0.ActivationState = ActivationState.DisableDeactivation; RigidBody fixedBody1 = LocalCreateRigidBody(0, trans, null); World.AddRigidBody(fixedBody1); Matrix frameInA = Matrix.Translation(0, 5, 0); Matrix frameInB = Matrix.Translation(0, 5, 0); //bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits bool useLinearReferenceFrameA = true;//use fixed frame A for linear llimits spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, frameInA, frameInB, useLinearReferenceFrameA); spSlider6Dof.LinearLowerLimit = lowerSliderLimit; spSlider6Dof.LinearUpperLimit = hiSliderLimit; //range should be small, otherwise singularities will 'explode' the constraint //spSlider6Dof.AngularLowerLimit = new Vector3(-1.5f,0,0); //spSlider6Dof.AngularUpperLimit = new Vector3(1.5f,0,0); //spSlider6Dof.AngularLowerLimit = new Vector3(0,0,0); //spSlider6Dof.AngularUpperLimit = new Vector3(0,0,0); spSlider6Dof.AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0); spSlider6Dof.AngularUpperLimit = new Vector3(1.5f, 0, 0); spSlider6Dof.TranslationalLimitMotor.EnableMotor[0] = true; spSlider6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0); spSlider6Dof.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0); World.AddConstraint(spSlider6Dof); spSlider6Dof.DebugDrawSize = 5; // create a door using hinge constraint attached to the world CollisionShape pDoorShape = new BoxShape(2.0f, 5.0f, 0.2f); CollisionShapes.Add(pDoorShape); RigidBody pDoorBody = LocalCreateRigidBody(1.0f, Matrix.Translation(-5.0f, -2.0f, 0.0f), pDoorShape); pDoorBody.ActivationState = ActivationState.DisableDeactivation; Vector3 btPivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside Vector3 btAxisA = Vector3.UnitY; // pointing upwards, aka Y-axis spDoorHinge = new HingeConstraint(pDoorBody, btPivotA, btAxisA); //spDoorHinge.SetLimit(0.0f, (float)Math.PI / 2); // test problem values //spDoorHinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f); //spDoorHinge.SetLimit(1, -1); //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI); //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f); //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits" spDoorHinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f); //spDoorHinge.SetLimit(0, 0); World.AddConstraint(spDoorHinge); spDoorHinge.DebugDrawSize = 5; RigidBody pDropBody = LocalCreateRigidBody(10.0f, Matrix.Translation(-5.0f, 2.0f, 0.0f), shape); // create a generic 6DOF constraint //RigidBody pBodyA = LocalCreateRigidBody(mass, Matrix.Translation(10.0f, 6.0f, 0), shape); RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), shape); //RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), null); pBodyA.ActivationState = ActivationState.DisableDeactivation; RigidBody pBodyB = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), shape); //RigidBody pBodyB = LocalCreateRigidBody(0, Matrix.Translation(0, 6, 0), shape); pBodyB.ActivationState = ActivationState.DisableDeactivation; frameInA = Matrix.Translation(-5, 0, 0); frameInB = Matrix.Translation(5, 0, 0); Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, true); //Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, false); pGen6DOF.LinearLowerLimit = new Vector3(-10, -2, -1); pGen6DOF.LinearUpperLimit = new Vector3(10, 2, 1); //pGen6DOF.LinearLowerLimit = new Vector3(-10, 0, 0); //pGen6DOF.LinearUpperLimit = new Vector3(10, 0, 0); //pGen6DOF.LinearLowerLimit = new Vector3(0, 0, 0); //pGen6DOF.LinearUpperLimit = new Vector3(0, 0, 0); //pGen6DOF.TranslationalLimitMotor.EnableMotor[0] = true; //pGen6DOF.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0); //pGen6DOF.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0); //pGen6DOF.AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0); //pGen6DOF.AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0); //pGen6DOF.AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI); //pGen6DOF.AngularUpperLimit = new Vector3(0, 0, (float)Math.PI); pGen6DOF.AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f); pGen6DOF.AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f); //pGen6DOF.AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f); //pGen6DOF.AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f); //pGen6DOF.AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f); //pGen6DOF.AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f); //pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f); //pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f); //pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, 0, 0); //pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0, 0); //pGen6DOF.AngularLowerLimit = new Vector3(0, -0.7f, 0); //pGen6DOF.AngularUpperLimit = new Vector3(0, 0.7f, 0); //pGen6DOF.AngularLowerLimit = new Vector3(-1, 0, 0); //pGen6DOF.AngularUpperLimit = new Vector3(1, 0, 0); // create a ConeTwist constraint pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), shape); //pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), shape); pBodyA.ActivationState = ActivationState.DisableDeactivation; pBodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), shape); //pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), shape); frameInA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2); frameInA *= Matrix.Translation(0, -5, 0); frameInB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2); frameInB *= Matrix.Translation(0, 5, 0); coneTwist = new ConeTwistConstraint(pBodyA, pBodyB, frameInA, frameInB); //coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f); //coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f); World.AddConstraint(coneTwist, true); coneTwist.DebugDrawSize = 5; // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver) RigidBody pBody = LocalCreateRigidBody(1.0f, Matrix.Identity, shape); pBody.ActivationState = ActivationState.DisableDeactivation; Vector3 PivotA = new Vector3(10.0f, 0.0f, 0.0f); btAxisA = new Vector3(0.0f, 0.0f, 1.0f); HingeConstraint pHinge = new HingeConstraint(pBody, btPivotA, btAxisA); //pHinge.EnableAngularMotor(true, -1.0f, 0.165f); // use for the old solver pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver World.AddConstraint(pHinge); pHinge.DebugDrawSize = 5; // create a universal joint using generic 6DOF constraint // create two rigid bodies // static bodyA (parent) on top: pBodyA = LocalCreateRigidBody(0, Matrix.Translation(20, 4, 0), shape); pBodyA.ActivationState = ActivationState.DisableDeactivation; // dynamic bodyB (child) below it : pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(20, 0, 0), shape); pBodyB.ActivationState = ActivationState.DisableDeactivation; // add some (arbitrary) data to build constraint frames Vector3 parentAxis = new Vector3(1, 0, 0); Vector3 childAxis = new Vector3(0, 0, 1); Vector3 anchor = new Vector3(20, 2, 0); UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, anchor, parentAxis, childAxis); pUniv.SetLowerLimit(-(float)Math.PI / 4, -(float)Math.PI / 4); pUniv.SetUpperLimit((float)Math.PI / 4, (float)Math.PI / 4); // add constraint to world World.AddConstraint(pUniv, true); // draw constraint frames and limits for debugging pUniv.DebugDrawSize = 5; World.AddConstraint(pGen6DOF, true); pGen6DOF.DebugDrawSize = 5; // create a generic 6DOF constraint with springs pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 16, 0), shape); pBodyA.ActivationState = ActivationState.DisableDeactivation; pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 16, 0), shape); pBodyB.ActivationState = ActivationState.DisableDeactivation; frameInA = Matrix.Translation(10, 0, 0); frameInB = Matrix.Identity; Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, frameInA, frameInB, true); pGen6DOFSpring.LinearUpperLimit = new Vector3(5, 0, 0); pGen6DOFSpring.LinearLowerLimit = new Vector3(-5, 0, 0); pGen6DOFSpring.AngularLowerLimit = new Vector3(0, 0, -1.5f); pGen6DOFSpring.AngularUpperLimit = new Vector3(0, 0, 1.5f); World.AddConstraint(pGen6DOFSpring, true); pGen6DOFSpring.DebugDrawSize = 5; pGen6DOFSpring.EnableSpring(0, true); pGen6DOFSpring.SetStiffness(0, 39.478f); pGen6DOFSpring.SetDamping(0, 0.5f); pGen6DOFSpring.EnableSpring(5, true); pGen6DOFSpring.SetStiffness(5, 39.478f); pGen6DOFSpring.SetDamping(0, 0.3f); pGen6DOFSpring.SetEquilibriumPoint(); // create a Hinge2 joint // create two rigid bodies // static bodyA (parent) on top: pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 4, 0), shape); pBodyA.ActivationState = ActivationState.DisableDeactivation; // dynamic bodyB (child) below it : pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, 0, 0), shape); pBodyB.ActivationState = ActivationState.DisableDeactivation; // add some data to build constraint frames parentAxis = new Vector3(0, 1, 0); childAxis = new Vector3(1, 0, 0); anchor = new Vector3(-20, 0, 0); Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, anchor, parentAxis, childAxis); pHinge2.SetLowerLimit(-(float)Math.PI / 4); pHinge2.SetUpperLimit((float)Math.PI / 4); // add constraint to world World.AddConstraint(pHinge2, true); // draw constraint frames and limits for debugging pHinge2.DebugDrawSize = 5; // create a Hinge joint between two dynamic bodies // create two rigid bodies // static bodyA (parent) on top: pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), shape); pBodyA.ActivationState = ActivationState.DisableDeactivation; // dynamic bodyB: pBodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), shape); pBodyB.ActivationState = ActivationState.DisableDeactivation; // add some data to build constraint frames Vector3 axisA = new Vector3(0, 1, 0); Vector3 axisB = new Vector3(0, 1, 0); Vector3 pivotA = new Vector3(-5, 0, 0); Vector3 pivotB = new Vector3(5, 0, 0); spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, pivotA, pivotB, axisA, axisB); spHingeDynAB.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4); // add constraint to world World.AddConstraint(spHingeDynAB, true); // draw constraint frames and limits for debugging spHingeDynAB.DebugDrawSize = 5; }
public override void InitializeDemo() { m_collisionConfiguration = new DefaultCollisionConfiguration(); m_dispatcher = new CollisionDispatcher(m_collisionConfiguration); IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000); IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000); m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false); m_constraintSolver = new SequentialImpulseConstraintSolver(); m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration); m_dynamicsWorld.SetDebugDrawer(m_debugDraw); SetCameraDistance(26f); //CollisionShape groundShape = new BoxShape(new IndexedVector3(50f, 40f, 50f)); CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0, 1, 0), 40); m_collisionShapes.Add(groundShape); IndexedMatrix groundTransform = IndexedMatrix.Identity; groundTransform._origin = new IndexedVector3(0, -56, 0); RigidBody groundBody = LocalCreateRigidBody(0, ref groundTransform, groundShape); CollisionShape shape = new BoxShape(new IndexedVector3(CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS)); m_collisionShapes.Add(shape); IndexedMatrix trans = IndexedMatrix.Identity; trans._origin = new IndexedVector3(0, 20, 0); float mass = 1f; #if true //point to point constraint (ball socket) //SEEMS OK { RigidBody body0 = LocalCreateRigidBody(mass, ref trans, shape); trans._origin = new IndexedVector3(2 * CUBE_HALF_EXTENTS, 20, 0); mass = 1f; RigidBody body1 = null; //localCreateRigidBody( mass,trans,shape); IndexedVector3 pivotInA = new IndexedVector3(CUBE_HALF_EXTENTS, -CUBE_HALF_EXTENTS, -CUBE_HALF_EXTENTS); IndexedVector3 axisInA = new IndexedVector3(0, 0, 1); IndexedVector3 pivotInB = body1 != null?body1.GetCenterOfMassTransform().Inverse() * (body0.GetCenterOfMassTransform() * (pivotInA)) : pivotInA; IndexedVector3 axisInB = body1 != null ? (body1.GetCenterOfMassTransform()._basis.Inverse() * (body1.GetCenterOfMassTransform()._basis *axisInA)) : body0.GetCenterOfMassTransform()._basis *axisInA; HingeConstraint hinge = new HingeConstraint(body0, ref pivotInA, ref axisInA, false); float targetVelocity = 1f; float maxMotorImpulse = 1.0f; hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse); m_dynamicsWorld.AddConstraint(hinge); //p2p); hinge.SetDbgDrawSize(5f); } #endif #if true //create a slider, using the generic D6 constraint // SEEMS OK { mass = 1f; IndexedVector3 sliderWorldPos = new IndexedVector3(0, 10, 0); IndexedVector3 sliderAxis = new IndexedVector3(1, 0, 0); float angle = 0f;//SIMD_RADS_PER_DEG * 10.f; IndexedBasisMatrix sliderOrientation = new IndexedBasisMatrix(Quaternion.CreateFromAxisAngle(sliderAxis.ToVector3(), angle)); trans = IndexedMatrix.Identity; trans._origin = sliderWorldPos; //trans.setBasis(sliderOrientation); sliderTransform = trans; d6body0 = LocalCreateRigidBody(mass, ref trans, shape); d6body0.SetActivationState(ActivationState.DISABLE_DEACTIVATION); RigidBody fixedBody1 = LocalCreateRigidBody(0, ref trans, null); m_dynamicsWorld.AddRigidBody(fixedBody1); IndexedMatrix frameInA, frameInB; frameInA = IndexedMatrix.Identity; frameInB = IndexedMatrix.Identity; frameInA._origin = new IndexedVector3(0, 5, 0); frameInB._origin = new IndexedVector3(0, 5, 0); // bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, ref frameInA, ref frameInB, useLinearReferenceFrameA); spSlider6Dof.SetLinearLowerLimit(ref lowerSliderLimit); spSlider6Dof.SetLinearUpperLimit(ref hiSliderLimit); //range should be small, otherwise singularities will 'explode' the constraint IndexedVector3 angularLower = new IndexedVector3(-1.5f, 0, 0); IndexedVector3 angularUpper = -angularLower; spSlider6Dof.SetAngularLowerLimit(ref angularLower); spSlider6Dof.SetAngularUpperLimit(ref angularUpper); // slider.setAngularLowerLimit(IndexedVector3(0,0,0)); // slider.setAngularUpperLimit(IndexedVector3(0,0,0)); spSlider6Dof.GetTranslationalLimitMotor().m_enableMotor[0] = true; spSlider6Dof.GetTranslationalLimitMotor().m_targetVelocity.X = -5.0f; spSlider6Dof.GetTranslationalLimitMotor().m_maxMotorForce.X = 0.1f; m_dynamicsWorld.AddConstraint(spSlider6Dof); spSlider6Dof.SetDbgDrawSize(5f); } #endif #if true { // create a door using hinge constraint attached to the world CollisionShape pDoorShape = new BoxShape(new IndexedVector3(2.0f, 5.0f, 0.2f)); m_collisionShapes.Add(pDoorShape); IndexedMatrix doorTrans = IndexedMatrix.Identity; doorTrans._origin = new IndexedVector3(-5.0f, -2.0f, 0.0f); RigidBody pDoorBody = LocalCreateRigidBody(1.0f, ref doorTrans, pDoorShape); pDoorBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedVector3 btPivotA = new IndexedVector3(10f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside IndexedVector3 btAxisA = new IndexedVector3(0.0f, 1.0f, 0.0f); // pointing upwards, aka Y-axis spDoorHinge = new HingeConstraint(pDoorBody, ref btPivotA, ref btAxisA, false); spDoorHinge.SetLimit(-MathUtil.SIMD_PI * 0.25f, MathUtil.SIMD_PI * 0.25f); m_dynamicsWorld.AddConstraint(spDoorHinge); spDoorHinge.SetDbgDrawSize(5.0f); } #endif #if true { // create a generic 6DOF constraint // SEEMS OK - But debug draw a bit wrong? IndexedMatrix tr = IndexedMatrix.Identity; tr._origin = new IndexedVector3(10f, 6f, 0f); //tr.getBasis().setEulerZYX(0,0,0); // RigidBody pBodyA = localCreateRigidBody( mass, tr, shape); RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape); // RigidBody pBodyA = localCreateRigidBody( 0.0, tr, 0); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); tr = IndexedMatrix.Identity; tr._origin = new IndexedVector3(0f, 6f, 0f); //tr.getBasis().setEulerZYX(0,0,0); RigidBody pBodyB = LocalCreateRigidBody(mass, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedMatrix frameInA, frameInB; frameInA = IndexedMatrix.CreateTranslation(-5, 0, 0); frameInB = IndexedMatrix.CreateTranslation(5, 0, 0); Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB, true); // btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, false); IndexedVector3 linearLower = new IndexedVector3(-10, -2, -1); pGen6DOF.SetLinearLowerLimit(ref linearLower); IndexedVector3 linearUpper = new IndexedVector3(10, 2, 1); pGen6DOF.SetLinearUpperLimit(ref linearUpper); // ? why again? //linearLower = new IndexedVector3(-10,0,0); //pGen6DOF.setLinearLowerLimit(ref linearLower); // pGen6DOF.setLinearUpperLimit(IndexedVector3(10., 0., 0.)); // pGen6DOF.setLinearLowerLimit(IndexedVector3(0., 0., 0.)); // pGen6DOF.setLinearUpperLimit(IndexedVector3(0., 0., 0.)); // pGen6DOF.getTranslationalLimitMotor().m_enableMotor[0] = true; // pGen6DOF.getTranslationalLimitMotor().m_targetVelocity[0] = 5.0f; // pGen6DOF.getTranslationalLimitMotor().m_maxMotorForce[0] = 0.1f; // pGen6DOF.setAngularLowerLimit(IndexedVector3(0., SIMD_HALF_PI*0.9, 0.)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0., -SIMD_HALF_PI*0.9, 0.)); // pGen6DOF.setAngularLowerLimit(IndexedVector3(0., 0., -SIMD_HALF_PI)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0., 0., SIMD_HALF_PI)); IndexedVector3 angularLower = new IndexedVector3(-MathUtil.SIMD_HALF_PI * 0.5f, -0.75f, -MathUtil.SIMD_HALF_PI * 0.8f); IndexedVector3 angularUpper = -angularLower; pGen6DOF.SetAngularLowerLimit(ref angularLower); pGen6DOF.SetAngularUpperLimit(ref angularUpper); // pGen6DOF.setAngularLowerLimit(IndexedVector3(0.f, -0.75, SIMD_HALF_PI * 0.8f)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0.f, 0.75, -SIMD_HALF_PI * 0.8f)); // pGen6DOF.setAngularLowerLimit(IndexedVector3(0.f, -SIMD_HALF_PI * 0.8f, SIMD_HALF_PI * 1.98f)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0.f, SIMD_HALF_PI * 0.8f, -SIMD_HALF_PI * 1.98f)); // pGen6DOF.setAngularLowerLimit(IndexedVector3(-0.75,-0.5, -0.5)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0.75,0.5, 0.5)); // pGen6DOF.setAngularLowerLimit(IndexedVector3(-0.75,0., 0.)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0.75,0., 0.)); m_dynamicsWorld.AddConstraint(pGen6DOF, true); pGen6DOF.SetDbgDrawSize(5.0f); } #endif #if true { // create a ConeTwist constraint IndexedMatrix tr = IndexedMatrix.CreateTranslation(-10, 5, 0); RigidBody pBodyA = LocalCreateRigidBody(1.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); tr = IndexedMatrix.CreateTranslation(-10, -5, 0); RigidBody pBodyB = LocalCreateRigidBody(0.0f, ref tr, shape); IndexedMatrix frameInA, frameInB; frameInA = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI); frameInA._origin = new IndexedVector3(0, -5, 0); frameInB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI); frameInB._origin = new IndexedVector3(0, 5, 0); ConeTwistConstraint pCT = new ConeTwistConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB); pCT.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_PI * 0.8f, 1.0f, 0.3f, 1.0f); // soft limit == hard limit m_dynamicsWorld.AddConstraint(pCT, true); pCT.SetDbgDrawSize(5.0f); } #endif #if true { // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver) // WORKS OK IndexedMatrix tr = IndexedMatrix.Identity; RigidBody pBody = LocalCreateRigidBody(1.0f, ref tr, shape); pBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedVector3 btPivotA = new IndexedVector3(10.0f, 0.0f, 0.0f); IndexedVector3 btAxisA = new IndexedVector3(0.0f, 0.0f, 1.0f); HingeConstraint pHinge = new HingeConstraint(pBody, ref btPivotA, ref btAxisA, false); // pHinge.enableAngularMotor(true, -1.0, 0.165); // use for the old solver pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver m_dynamicsWorld.AddConstraint(pHinge); pHinge.SetDbgDrawSize(5.0f); } #endif #if true { // WORKS OK // create a universal joint using generic 6DOF constraint // create two rigid bodies // static bodyA (parent) on top: IndexedMatrix tr = IndexedMatrix.CreateTranslation(20, 4, 0); RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // dynamic bodyB (child) below it : tr = IndexedMatrix.CreateTranslation(20, 0, 0); RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // add some (arbitrary) data to build constraint frames IndexedVector3 parentAxis = new IndexedVector3(1.0f, 0.0f, 0.0f); IndexedVector3 childAxis = new IndexedVector3(0.0f, 0.0f, 1.0f); IndexedVector3 anchor = new IndexedVector3(20.0f, 2.0f, 0.0f); UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, ref anchor, ref parentAxis, ref childAxis); pUniv.SetLowerLimit(-MathUtil.SIMD_HALF_PI * 0.5f, -MathUtil.SIMD_HALF_PI * 0.5f); pUniv.SetUpperLimit(MathUtil.SIMD_HALF_PI * 0.5f, MathUtil.SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld.AddConstraint(pUniv, true); // draw constraint frames and limits for debugging pUniv.SetDbgDrawSize(5.0f); } #endif #if true // WORKS OK { // create a generic 6DOF constraint with springs IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, 16f, 0f); //tr.setIdentity(); //tr.setOrigin(btVector3(btScalar(-20.), btScalar(16.), btScalar(0.))); //tr.getBasis().setEulerZYX(0,0,0); RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); //tr.setIdentity(); //tr.setOrigin(btVector3(btScalar(-10.), btScalar(16.), btScalar(0.))); //tr.getBasis().setEulerZYX(0,0,0); tr = IndexedMatrix.CreateTranslation(-10, 16, 0); RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedMatrix frameInA = IndexedMatrix.CreateTranslation(10f, 0f, 0f); IndexedMatrix frameInB = IndexedMatrix.CreateTranslation(0f, 0f, 0f); Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB, true); pGen6DOFSpring.SetLinearUpperLimit(new IndexedVector3(5f, 0f, 0f)); pGen6DOFSpring.SetLinearLowerLimit(new IndexedVector3(-5f, 0f, 0f)); pGen6DOFSpring.SetAngularLowerLimit(new IndexedVector3(0f, 0f, -1.5f)); pGen6DOFSpring.SetAngularUpperLimit(new IndexedVector3(0f, 0f, 1.5f)); m_dynamicsWorld.AddConstraint(pGen6DOFSpring, true); pGen6DOFSpring.SetDbgDrawSize(5.0f); pGen6DOFSpring.EnableSpring(0, true); pGen6DOFSpring.SetStiffness(0, 39.478f); pGen6DOFSpring.SetDamping(0, 0.5f); pGen6DOFSpring.EnableSpring(5, true); pGen6DOFSpring.SetStiffness(5, 39.478f); pGen6DOFSpring.SetDamping(0, 0.3f); pGen6DOFSpring.SetEquilibriumPoint(); } #endif #if true { // WORKS OK // create a Hinge2 joint // create two rigid bodies // static bodyA (parent) on top: IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, 4f, 0f); RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // dynamic bodyB (child) below it : tr = IndexedMatrix.CreateTranslation(-20f, 0f, 0f); RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // add some data to build constraint frames IndexedVector3 parentAxis = new IndexedVector3(0.0f, 1.0f, 0.0f); IndexedVector3 childAxis = new IndexedVector3(1.0f, 0.0f, 0.0f); IndexedVector3 anchor = new IndexedVector3(-20.0f, 0.0f, 0.0f); Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, ref anchor, ref parentAxis, ref childAxis); pHinge2.SetLowerLimit(-MathUtil.SIMD_HALF_PI * 0.5f); pHinge2.SetUpperLimit(MathUtil.SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld.AddConstraint(pHinge2, true); // draw constraint frames and limits for debugging pHinge2.SetDbgDrawSize(5.0f); } #endif #if true { // WORKS OK // create a Hinge joint between two dynamic bodies // create two rigid bodies // static bodyA (parent) on top: IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, -2f, 0f); RigidBody pBodyA = LocalCreateRigidBody(1.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // dynamic bodyB: tr = IndexedMatrix.CreateTranslation(-30f, -2f, 0f); RigidBody pBodyB = LocalCreateRigidBody(10.0f, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // add some data to build constraint frames IndexedVector3 axisA = new IndexedVector3(0.0f, 1.0f, 0.0f); IndexedVector3 axisB = new IndexedVector3(0.0f, 1.0f, 0.0f); IndexedVector3 pivotA = new IndexedVector3(-5.0f, 0.0f, 0.0f); IndexedVector3 pivotB = new IndexedVector3(5.0f, 0.0f, 0.0f); spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, ref pivotA, ref pivotB, ref axisA, ref axisB); spHingeDynAB.SetLimit(-MathUtil.SIMD_HALF_PI * 0.5f, MathUtil.SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld.AddConstraint(spHingeDynAB, true); // draw constraint frames and limits for debugging spHingeDynAB.SetDbgDrawSize(5.0f); } #endif }
public IGeneric6DofConstraintImp AddGeneric6DofConstraint(IRigidBodyImp rigidBodyA, IRigidBodyImp rigidBodyB, float4x4 frameInA, float4x4 frameInB, bool useReferenceFrameA = false) { var rigidBodyAImp = (RigidBodyImp)rigidBodyA; var btRigidBodyA = rigidBodyAImp._rbi; var rigidBodyBImp = (RigidBodyImp)rigidBodyB; var btRigidBodyB = rigidBodyAImp._rbi; Matrix matrixA = Translater.Float4X4ToBtMatrix(frameInA); Matrix matrixB = Translater.Float4X4ToBtMatrix(frameInB); var btGeneric6DofConstraint = new Generic6DofConstraint(btRigidBodyA, btRigidBodyB, matrixA, matrixB, useReferenceFrameA); BtWorld.AddConstraint(btGeneric6DofConstraint); var retval = new Generic6DofConstraintImp(); retval._g6dofci = btGeneric6DofConstraint; btGeneric6DofConstraint.UserObject = retval; return retval; }
public ConstraintDemo() { CollisionShape shape = new BoxShape(new Vector3(HalfExtents, HalfExtents, HalfExtents)); Matrix trans = Matrix.Identity; trans.Translation = new Vector3(0, 20, 0); float mass = 1f; //Point to Point constraint (ball socket) { RigidBody bodyA = CreateRigidBody(mass, trans, shape); trans.Translation = new Vector3(2 * HalfExtents, 20, 0); mass = 1f; RigidBody bodyB = null; //RigidBody bodyB = CreateRigidBody(mass, trans, shape); //bodyB.ActivationState = ActivationState.DisableDeactivation; //bodyB.SetDamping(0.3f, 0.3f); Vector3 pivotInA = new Vector3(HalfExtents, -HalfExtents, -HalfExtents); Vector3 axisInA = new Vector3(0, 0, 1); Vector3 pivotInB = bodyB != null?MathHelper.MatrixToVector(MathHelper.InvertMatrix(bodyB.CenterOfMassTransform), MathHelper.MatrixToVector(bodyA.CenterOfMassTransform, pivotInA)) : pivotInA; Vector3 axisInB = bodyB != null? Vector3.TransformNormal(Vector3.TransformNormal(axisInA, bodyB.CenterOfMassTransform), MathHelper.InvertMatrix(bodyB.CenterOfMassTransform)) : Vector3.TransformNormal(axisInA, bodyA.CenterOfMassTransform); //TypedConstraint p2p = new Point2PointConstraint(bodyA, bodyB, pivotInA, pivotInB); //TypedConstraint hinge = new HingeConstraint(bodyA, bodyB, pivotInA, pivotInB, axisInA, axisInB); HingeConstraint hinge = new HingeConstraint(bodyA, pivotInA, axisInA); //use zero targetVelocity and a small maxMotorImpulse to simulate joint friction //float targetVelocity = 0.0f; //float maxMotorImpulse = 0.01; float targetVelocity = 1.0f; float maxMotorImpulse = 1.0f; hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse); PhysicsWorld.AddConstraint(hinge); } // create a slider, using the generic D6 constraint { mass = 1f; Vector3 sliderWorldPos = new Vector3(0, 10, 0); Vector3 sliderAxis = new Vector3(1, 0, 0); float angle = 0; Matrix sliderOrientation = Matrix.CreateFromQuaternion(new Quaternion(sliderAxis, angle)); trans = Matrix.Identity; trans.Translation = sliderWorldPos; //trans.setBasis(sliderOrientation); _sliderTransform = trans; _d6BodyA = CreateRigidBody(mass, trans, shape); _d6BodyA.ActivationState = ActivationState.DisableDeactivation; RigidBody fixedBody1 = CreateRigidBody(0, trans, null); Matrix frameInA, frameInB; frameInA = Matrix.Identity; frameInB = Matrix.Identity; Generic6DofConstraint slider = new Generic6DofConstraint(_d6BodyA, fixedBody1, frameInA, frameInB); slider.SetLinearLowerLimit(_lowerSliderLimit); slider.SetLinearUpperLimit(_hiSliderLimit); //range should be small, otherwise singularities will 'explode' the constraint slider.SetAngularLowerLimit(new Vector3(10, 0, 0)); slider.SetAngularUpperLimit(new Vector3(0, 0, 0)); PhysicsWorld.AddConstraint(slider); } }
public virtual void OnHandleInput() { if (Input.KeysPressed.Count != 0) { switch (Input.KeysPressed[0]) { case Keys.Escape: case Keys.Q: Graphics.Form.Close(); return; case Keys.F1: MessageBox.Show( "Move using WASD + shift\n" + "Left click - point camera\n" + "Right click - pick up an object using a Point2PointConstraint\n" + "Right click + shift - pick up an object using a fixed Generic6DofConstraint\n" + "Space - shoot box\n" + "Q - quit\n" + Graphics.InfoText, "Help"); // Key release won't be captured Input.KeysDown.Remove(Keys.F1); break; case Keys.F3: IsDebugDrawEnabled = !IsDebugDrawEnabled; break; case Keys.F8: Input.ClearKeyCache(); GraphicsLibraryManager.ExitWithReload = true; Graphics.Form.Close(); break; case Keys.F11: Graphics.IsFullScreen = !Graphics.IsFullScreen; break; case (Keys.Control | Keys.F): const int maxSerializeBufferSize = 1024 * 1024 * 5; using (var serializer = new DefaultSerializer(maxSerializeBufferSize)) { World.Serialize(serializer); byte[] dataBytes = new byte[serializer.CurrentBufferSize]; System.Runtime.InteropServices.Marshal.Copy(serializer.BufferPointer, dataBytes, 0, dataBytes.Length); using (var file = new System.IO.FileStream("world.bullet", System.IO.FileMode.Create)) { file.Write(dataBytes, 0, dataBytes.Length); } } break; case Keys.G: //shadowsEnabled = !shadowsEnabled; break; case Keys.Space: ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView)); break; case Keys.Return: ClientResetScene(); break; } } if (Input.MousePressed != MouseButtons.None) { Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView); if (Input.MousePressed == MouseButtons.Right) { if (World != null) { Vector3 rayFrom = Freelook.Eye; ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo); World.RayTest(ref rayFrom, ref rayTo, rayCallback); if (rayCallback.HasHit) { Vector3 pickPos = rayCallback.HitPointWorld; RigidBody body = rayCallback.CollisionObject as RigidBody; if (body != null) { if (!(body.IsStaticObject || body.IsKinematicObject)) { pickedBody = body; pickedBody.ActivationState = ActivationState.DisableDeactivation; Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform)); if (Input.KeysDown.Contains(Keys.ShiftKey)) { Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false) { LinearLowerLimit = Vector3.Zero, LinearUpperLimit = Vector3.Zero, AngularLowerLimit = Vector3.Zero, AngularUpperLimit = Vector3.Zero }; World.AddConstraint(dof6); pickConstraint = dof6; dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5); } else { Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot); World.AddConstraint(p2p); pickConstraint = p2p; p2p.Setting.ImpulseClamp = 30; //very weak constraint for picking p2p.Setting.Tau = 0.001f; /* * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 0); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 1); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 2); */ } } } else { MultiBodyLinkCollider multiCol = rayCallback.CollisionObject as MultiBodyLinkCollider; if (multiCol != null && multiCol.MultiBody != null) { MultiBody mb = multiCol.MultiBody; prevCanSleep = mb.CanSleep; mb.CanSleep = false; Vector3 pivotInA = mb.WorldPosToLocal(multiCol.Link, pickPos); MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(mb, multiCol.Link, null, pivotInA, pickPos); p2p.MaxAppliedImpulse = 2; (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p); pickingMultiBodyPoint2Point = p2p; } } oldPickingDist = (pickPos - rayFrom).Length; } rayCallback.Dispose(); } } } else if (Input.MouseReleased == MouseButtons.Right) { RemovePickingConstraint(); } // Mouse movement if (Input.MouseDown == MouseButtons.Right) { MovePickedBody(); } }
protected virtual void OnHandleInput() { if (Input.KeysPressed.Count != 0) { Keys key = Input.KeysPressed[0]; switch (key) { case Keys.Escape: case Keys.Q: Form.Close(); return; case Keys.F3: //IsDebugDrawEnabled = !IsDebugDrawEnabled; break; case Keys.F11: //ToggleFullScreen(); break; case Keys.G: shadowsEnabled = !shadowsEnabled; break; case Keys.Space: PhysicsContext.ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView)); break; } } if (Input.MousePressed != MouseButtons.None) { Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView); if (Input.MousePressed == MouseButtons.Right) { if (PhysicsContext.World != null) { Vector3 rayFrom = Freelook.Eye; ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(rayFrom, rayTo); PhysicsContext.World.RayTest(rayFrom, rayTo, rayCallback); if (rayCallback.HasHit) { RigidBody body = rayCallback.CollisionObject as RigidBody; if (body != null) { if (!(body.IsStaticObject || body.IsKinematicObject)) { pickedBody = body; pickedBody.ActivationState = ActivationState.DisableDeactivation; Vector3 pickPos = rayCallback.HitPointWorld; Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform)); if (use6Dof) { Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false); dof6.LinearLowerLimit = Vector3.Zero; dof6.LinearUpperLimit = Vector3.Zero; dof6.AngularLowerLimit = Vector3.Zero; dof6.AngularUpperLimit = Vector3.Zero; PhysicsContext.World.AddConstraint(dof6); pickConstraint = dof6; dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5); } else { Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot); PhysicsContext.World.AddConstraint(p2p); pickConstraint = p2p; p2p.Setting.ImpulseClamp = 30; //very weak constraint for picking p2p.Setting.Tau = 0.001f; /* * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 0); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 1); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 2); */ } use6Dof = !use6Dof; oldPickingDist = (pickPos - rayFrom).Length(); } } } } } } else if (Input.MouseReleased == MouseButtons.Right) { RemovePickingConstraint(); } // Mouse movement if (Input.MouseDown == MouseButtons.Right) { if (pickConstraint != null) { Vector3 newRayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView); if (pickConstraint.ConstraintType == TypedConstraintType.D6) { Generic6DofConstraint pickCon = pickConstraint as Generic6DofConstraint; //keep it at the same picking distance Vector3 rayFrom = Freelook.Eye; Vector3 dir = newRayTo - rayFrom; dir.Normalize(); dir *= oldPickingDist; Vector3 newPivotB = rayFrom + dir; Matrix tempFrameOffsetA = pickCon.FrameOffsetA; tempFrameOffsetA.M41 = newPivotB.X; tempFrameOffsetA.M42 = newPivotB.Y; tempFrameOffsetA.M43 = newPivotB.Z; pickCon.FrameOffsetA = tempFrameOffsetA; } else { Point2PointConstraint pickCon = pickConstraint as Point2PointConstraint; //keep it at the same picking distance Vector3 rayFrom = Freelook.Eye; Vector3 dir = newRayTo - rayFrom; dir.Normalize(); dir *= oldPickingDist; pickCon.PivotInB = rayFrom + dir; } } } }
private void HandleMouseInput() { if (Input.MousePressed != MouseButtons.None) { Vector3 rayTo = GetRayTo(Input.MousePoint, FreeLook.Eye, FreeLook.Target, Graphics.FieldOfView); if (Input.MousePressed == MouseButtons.Right) { Vector3 rayFrom = FreeLook.Eye; var rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo); Simulation.World.RayTestRef(ref rayFrom, ref rayTo, rayCallback); if (rayCallback.HasHit) { Vector3 pickPos = rayCallback.HitPointWorld; RigidBody body = rayCallback.CollisionObject as RigidBody; if (body != null) { if (!(body.IsStaticObject || body.IsKinematicObject)) { pickedBody = body; pickedBody.ActivationState = ActivationState.DisableDeactivation; Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform)); if (Input.KeysDown.Contains(Keys.ShiftKey)) { Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false) { LinearLowerLimit = Vector3.Zero, LinearUpperLimit = Vector3.Zero, AngularLowerLimit = Vector3.Zero, AngularUpperLimit = Vector3.Zero }; Simulation.World.AddConstraint(dof6); pickConstraint = dof6; dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5); } else { Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot); Simulation.World.AddConstraint(p2p); pickConstraint = p2p; p2p.Setting.ImpulseClamp = 30; //very weak constraint for picking p2p.Setting.Tau = 0.001f; /* * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 0); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 1); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 2); */ } } } else { var multiCol = rayCallback.CollisionObject as MultiBodyLinkCollider; if (multiCol != null && multiCol.MultiBody != null) { MultiBody mb = multiCol.MultiBody; prevCanSleep = mb.CanSleep; mb.CanSleep = false; Vector3 pivotInA = mb.WorldPosToLocal(multiCol.Link, pickPos); var p2p = new MultiBodyPoint2Point(mb, multiCol.Link, null, pivotInA, pickPos); p2p.MaxAppliedImpulse = 2; (Simulation.World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p); pickingMultiBodyPoint2Point = p2p; } } oldPickingDist = (pickPos - rayFrom).Length; } rayCallback.Dispose(); } } else if (Input.MouseReleased == MouseButtons.Right) { RemovePickingConstraint(); } // Mouse movement if (Input.MouseDown == MouseButtons.Right) { MovePickedBody(); } }
//called by Physics World just before constraint is added to world. //the current constraint properties are used to rebuild the constraint. internal override bool _BuildConstraint() { BPhysicsWorld world = BPhysicsWorld.Get(); if (m_constraintPtr != null) { if (m_isInWorld && world != null) { m_isInWorld = false; world.RemoveConstraint(m_constraintPtr); } } BRigidBody targetRigidBodyA = GetComponent <BRigidBody>(); if (targetRigidBodyA == null) { Debug.LogError("B6DOFConstraint needs to be added to a component with a BRigidBody."); return(false); } if (!targetRigidBodyA.isInWorld) { world.AddRigidBody(targetRigidBodyA); } RigidBody rba = (RigidBody)targetRigidBodyA.GetCollisionObject(); if (rba == null) { Debug.LogError("Constraint could not get bullet RigidBody from target rigid body"); return(false); } if (m_constraintType == ConstraintType.constrainToAnotherBody) { if (m_otherRigidBody == null) { Debug.LogError("Other Rigid Body is not set."); return(false); } if (!m_otherRigidBody.isInWorld) { world.AddRigidBody(m_otherRigidBody); return(false); } RigidBody rbb = (RigidBody)m_otherRigidBody.GetCollisionObject(); if (rbb == null) { Debug.LogError("Constraint could not get bullet RigidBody from target rigid body"); return(false); } BM.Matrix frameInA, frameInOther; string errormsg = ""; if (CreateFramesA_B(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, out frameInA, out frameInOther, ref errormsg)) { m_constraintPtr = new Generic6DofConstraint(rbb, rba, frameInOther, frameInA, true); } else { Debug.LogError(errormsg); return(false); } } else { //TODO think about this BM.Matrix frameInA = BulletSharp.Math.Matrix.Identity; m_constraintPtr = new Generic6DofConstraint(rba, frameInA, false); } m_constraintPtr.Userobject = this; Generic6DofConstraint sl = (Generic6DofConstraint)m_constraintPtr; sl.LinearLowerLimit = m_linearLimitLower.ToBullet(); sl.LinearUpperLimit = m_linearLimitUpper.ToBullet(); sl.AngularLowerLimit = m_angularLimitLowerRadians.ToBullet(); sl.AngularUpperLimit = m_angularLimitUpperRadians.ToBullet(); sl.TranslationalLimitMotor.TargetVelocity = m_motorLinearTargetVelocity.ToBullet(); sl.TranslationalLimitMotor.MaxMotorForce = m_motorLinearMaxMotorForce.ToBullet(); sl.BreakingImpulseThreshold = m_breakingImpulseThreshold; sl.DebugDrawSize = m_debugDrawSize; return(true); }
public virtual void OnHandleInput() { if (Input.KeysPressed.Count != 0) { switch (Input.KeysPressed[0]) { case Keys.Escape: case Keys.Q: Graphics.Form.Close(); return; case Keys.F3: IsDebugDrawEnabled = !IsDebugDrawEnabled; break; case Keys.F8: Input.ClearKeyCache(); GraphicsLibraryManager.ExitWithReload = true; Graphics.Form.Close(); break; case Keys.F11: Graphics.IsFullScreen = !Graphics.IsFullScreen; break; case Keys.G: //shadowsEnabled = !shadowsEnabled; break; case Keys.Space: ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView)); break; case Keys.Return: ClientResetScene(); break; } } if (Input.MousePressed != MouseButtons.None) { Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView); if (Input.MousePressed == MouseButtons.Right) { if (_world != null) { Vector3 rayFrom = Freelook.Eye; ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo); _world.RayTest(ref rayFrom, ref rayTo, rayCallback); if (rayCallback.HasHit) { Vector3 pickPos = rayCallback.HitPointWorld; RigidBody body = rayCallback.CollisionObject as RigidBody; if (body != null) { if (!(body.IsStaticObject || body.IsKinematicObject)) { pickedBody = body; pickedBody.ActivationState = ActivationState.DisableDeactivation; Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform)); if (Input.KeysDown.Contains(Keys.ShiftKey)) { Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false) { LinearLowerLimit = Vector3.Zero, LinearUpperLimit = Vector3.Zero, AngularLowerLimit = Vector3.Zero, AngularUpperLimit = Vector3.Zero }; _world.AddConstraint(dof6); pickConstraint = dof6; dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5); } else { Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot); _world.AddConstraint(p2p); pickConstraint = p2p; p2p.Setting.ImpulseClamp = 30; //very weak constraint for picking p2p.Setting.Tau = 0.001f; /* * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 0); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 1); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 2); */ } } } else { MultiBodyLinkCollider multiCol = rayCallback.CollisionObject as MultiBodyLinkCollider; if (multiCol != null && multiCol.MultiBody != null) { MultiBody mb = multiCol.MultiBody; prevCanSleep = mb.CanSleep; mb.CanSleep = false; Vector3 pivotInA = mb.WorldPosToLocal(multiCol.Link, pickPos); MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(mb, multiCol.Link, null, pivotInA, pickPos); p2p.MaxAppliedImpulse = 2; (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p); pickingMultiBodyPoint2Point = p2p; } } oldPickingDist = (pickPos - rayFrom).Length; } rayCallback.Dispose(); } } } else if (Input.MouseReleased == MouseButtons.Right) { RemovePickingConstraint(); } // Mouse movement if (Input.MouseDown == MouseButtons.Right) { MovePickedBody(); } }
//------------------------------------------------------ // Picking //------------------------------------------------------ public void pickObject(Vector3 rayFrom, Vector3 rayTo, bool use6Dof) { ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo); _physics_world.world.RayTest(rayFrom, rayTo, rayCallback); if (rayCallback.HasHit) { Vector3 pickPos = rayCallback.HitPointWorld; RigidBody body = rayCallback.CollisionObject as RigidBody; if (body != null) { if (!(body.IsStaticObject || body.IsKinematicObject)) { _picked_body = body; _picked_body.ActivationState = ActivationState.DisableDeactivation; Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform)); if (use6Dof) { Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false); dof6.LinearLowerLimit = Vector3.Zero; dof6.LinearUpperLimit = Vector3.Zero; dof6.AngularLowerLimit = Vector3.Zero; dof6.AngularUpperLimit = Vector3.Zero; _physics_world.world.AddConstraint(dof6); _pick_constraint = dof6; dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5); } else { Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot); _physics_world.world.AddConstraint(p2p); _pick_constraint = p2p; p2p.Setting.ImpulseClamp = 30; //very weak constraint for picking p2p.Setting.Tau = 0.001f; /* * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 0); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 1); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 2); */ } _picking_distance_original = (pickPos - rayFrom).Length; _picking_distance_current = _picking_distance_original; } } } }
protected override void OnInitializePhysics() { SetupEmptyDynamicsWorld(); CollisionShape groundShape = new BoxShape(50, 1, 50); //CollisionShape groundShape = new StaticPlaneShape(Vector3.UnitY, 40); CollisionShapes.Add(groundShape); RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape); body.UserObject = "Ground"; CollisionShape shape = new BoxShape(new Vector3(CubeHalfExtents)); CollisionShapes.Add(shape); const float THETA = (float)Math.PI / 4.0f; float L_1 = 2 - (float)Math.Tan(THETA); float L_2 = 1 / (float)Math.Cos(THETA); float RATIO = L_2 / L_1; RigidBody bodyA; RigidBody bodyB; CollisionShape cylA = new CylinderShape(0.2f, 0.25f, 0.2f); CollisionShape cylB = new CylinderShape(L_1, 0.025f, L_1); CompoundShape cyl0 = new CompoundShape(); cyl0.AddChildShape(Matrix.Identity, cylA); cyl0.AddChildShape(Matrix.Identity, cylB); float mass = 6.28f; Vector3 localInertia; cyl0.CalculateLocalInertia(mass, out localInertia); RigidBodyConstructionInfo ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia); ci.StartWorldTransform = Matrix.Translation(-8, 1, -8); body = new RigidBody(ci); //1,0,cyl0,localInertia); World.AddRigidBody(body); body.LinearFactor = Vector3.Zero; body.AngularFactor = new Vector3(0, 1, 0); bodyA = body; cylA = new CylinderShape(0.2f, 0.26f, 0.2f); cylB = new CylinderShape(L_2, 0.025f, L_2); cyl0 = new CompoundShape(); cyl0.AddChildShape(Matrix.Identity, cylA); cyl0.AddChildShape(Matrix.Identity, cylB); mass = 6.28f; cyl0.CalculateLocalInertia(mass, out localInertia); ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia); Quaternion orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA); ci.StartWorldTransform = Matrix.RotationQuaternion(orn) * Matrix.Translation(-10, 2, -8); body = new RigidBody(ci);//1,0,cyl0,localInertia); body.LinearFactor = Vector3.Zero; HingeConstraint hinge = new HingeConstraint(body, Vector3.Zero, new Vector3(0, 1, 0), true); World.AddConstraint(hinge); bodyB = body; body.AngularVelocity = new Vector3(0, 3, 0); World.AddRigidBody(body); Vector3 axisA = new Vector3(0, 1, 0); Vector3 axisB = new Vector3(0, 1, 0); orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA); Matrix mat = Matrix.RotationQuaternion(orn); axisB = new Vector3(mat.M21, mat.M22, mat.M23); GearConstraint gear = new GearConstraint(bodyA, bodyB, axisA, axisB, RATIO); World.AddConstraint(gear, true); mass = 1.0f; RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), shape); RigidBody body1 = null;//LocalCreateRigidBody(mass, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), shape); //RigidBody body1 = LocalCreateRigidBody(0, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), null); //body1.ActivationState = ActivationState.DisableDeactivation; //body1.SetDamping(0.3f, 0.3f); Vector3 pivotInA = new Vector3(CubeHalfExtents, -CubeHalfExtents, -CubeHalfExtents); Vector3 axisInA = new Vector3(0, 0, 1); Vector3 pivotInB; if (body1 != null) { Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform; pivotInB = Vector3.TransformCoordinate(pivotInA, transform); } else { pivotInB = pivotInA; } Vector3 axisInB; if (body1 != null) { Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform; axisInB = Vector3.TransformCoordinate(axisInA, transform); } else { axisInB = Vector3.TransformCoordinate(axisInA, body0.CenterOfMassTransform); } #if P2P { TypedConstraint p2p = new Point2PointConstraint(body0, pivotInA); //TypedConstraint p2p = new Point2PointConstraint(body0, body1, pivotInA, pivotInB); //TypedConstraint hinge = new HingeConstraint(body0, body1, pivotInA, pivotInB, axisInA, axisInB); World.AddConstraint(p2p); p2p.DebugDrawSize = 5; } #else { hinge = new HingeConstraint(body0, pivotInA, axisInA); //use zero targetVelocity and a small maxMotorImpulse to simulate joint friction //float targetVelocity = 0.f; //float maxMotorImpulse = 0.01; const float targetVelocity = 1.0f; const float maxMotorImpulse = 1.0f; hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse); World.AddConstraint(hinge); hinge.DebugDrawSize = 5; } #endif RigidBody pRbA1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape); //RigidBody pRbA1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape); pRbA1.ActivationState = ActivationState.DisableDeactivation; // add dynamic rigid body B1 RigidBody pRbB1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape); //RigidBody pRbB1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape); pRbB1.ActivationState = ActivationState.DisableDeactivation; // create slider constraint between A1 and B1 and add it to world SliderConstraint spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, true); //spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, false); spSlider1.LowerLinearLimit = -15.0f; spSlider1.UpperLinearLimit = -5.0f; spSlider1.LowerLinearLimit = 5.0f; spSlider1.UpperLinearLimit = 15.0f; spSlider1.LowerLinearLimit = -10.0f; spSlider1.UpperLinearLimit = -10.0f; spSlider1.LowerAngularLimit = -(float)Math.PI / 3.0f; spSlider1.UpperAngularLimit = (float)Math.PI / 3.0f; World.AddConstraint(spSlider1, true); spSlider1.DebugDrawSize = 5.0f; //create a slider, using the generic D6 constraint Vector3 sliderWorldPos = new Vector3(0, 10, 0); Vector3 sliderAxis = Vector3.UnitX; const float angle = 0; //SIMD_RADS_PER_DEG * 10.f; Matrix trans = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(sliderWorldPos); d6body0 = LocalCreateRigidBody(mass, trans, shape); d6body0.ActivationState = ActivationState.DisableDeactivation; RigidBody fixedBody1 = LocalCreateRigidBody(0, trans, null); World.AddRigidBody(fixedBody1); Matrix frameInA = Matrix.Translation(0, 5, 0); Matrix frameInB = Matrix.Translation(0, 5, 0); //bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits const bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, frameInA, frameInB, useLinearReferenceFrameA) { LinearLowerLimit = lowerSliderLimit, LinearUpperLimit = hiSliderLimit, //range should be small, otherwise singularities will 'explode' the constraint //AngularLowerLimit = new Vector3(-1.5f,0,0), //AngularUpperLimit = new Vector3(1.5f,0,0), //AngularLowerLimit = new Vector3(0,0,0), //AngularUpperLimit = new Vector3(0,0,0), AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0), AngularUpperLimit = new Vector3(1.5f, 0, 0) }; //spSlider6Dof.TranslationalLimitMotor.EnableMotor[0] = true; spSlider6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0); spSlider6Dof.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0); World.AddConstraint(spSlider6Dof); spSlider6Dof.DebugDrawSize = 5; // create a door using hinge constraint attached to the world CollisionShape pDoorShape = new BoxShape(2.0f, 5.0f, 0.2f); CollisionShapes.Add(pDoorShape); RigidBody pDoorBody = LocalCreateRigidBody(1.0f, Matrix.Translation(-5.0f, -2.0f, 0.0f), pDoorShape); pDoorBody.ActivationState = ActivationState.DisableDeactivation; Vector3 btPivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside Vector3 btAxisA = Vector3.UnitY; // pointing upwards, aka Y-axis spDoorHinge = new HingeConstraint(pDoorBody, btPivotA, btAxisA); //spDoorHinge.SetLimit(0.0f, (float)Math.PI / 2); // test problem values //spDoorHinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f); //spDoorHinge.SetLimit(1, -1); //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI); //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f); //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits" spDoorHinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f); //spDoorHinge.SetLimit(0, 0); World.AddConstraint(spDoorHinge); spDoorHinge.DebugDrawSize = 5; RigidBody pDropBody = LocalCreateRigidBody(10.0f, Matrix.Translation(-5.0f, 2.0f, 0.0f), shape); // create a generic 6DOF constraint //RigidBody pBodyA = LocalCreateRigidBody(mass, Matrix.Translation(10.0f, 6.0f, 0), shape); RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), shape); //RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), null); pBodyA.ActivationState = ActivationState.DisableDeactivation; RigidBody pBodyB = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), shape); //RigidBody pBodyB = LocalCreateRigidBody(0, Matrix.Translation(0, 6, 0), shape); pBodyB.ActivationState = ActivationState.DisableDeactivation; frameInA = Matrix.Translation(-5, 0, 0); frameInB = Matrix.Translation(5, 0, 0); Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, true); //Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, false); pGen6DOF.LinearLowerLimit = new Vector3(-10, -2, -1); pGen6DOF.LinearUpperLimit = new Vector3(10, 2, 1); //pGen6DOF.LinearLowerLimit = new Vector3(-10, 0, 0); //pGen6DOF.LinearUpperLimit = new Vector3(10, 0, 0); //pGen6DOF.LinearLowerLimit = new Vector3(0, 0, 0); //pGen6DOF.LinearUpperLimit = new Vector3(0, 0, 0); //pGen6DOF.TranslationalLimitMotor.EnableMotor[0] = true; //pGen6DOF.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0); //pGen6DOF.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0); //pGen6DOF.AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0); //pGen6DOF.AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0); //pGen6DOF.AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI); //pGen6DOF.AngularUpperLimit = new Vector3(0, 0, (float)Math.PI); pGen6DOF.AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f); pGen6DOF.AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f); //pGen6DOF.AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f); //pGen6DOF.AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f); //pGen6DOF.AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f); //pGen6DOF.AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f); //pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f); //pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f); //pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, 0, 0); //pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0, 0); //pGen6DOF.AngularLowerLimit = new Vector3(0, -0.7f, 0); //pGen6DOF.AngularUpperLimit = new Vector3(0, 0.7f, 0); //pGen6DOF.AngularLowerLimit = new Vector3(-1, 0, 0); //pGen6DOF.AngularUpperLimit = new Vector3(1, 0, 0); // create a ConeTwist constraint pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), shape); //pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), shape); pBodyA.ActivationState = ActivationState.DisableDeactivation; pBodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), shape); //pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), shape); frameInA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2); frameInA *= Matrix.Translation(0, -5, 0); frameInB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2); frameInB *= Matrix.Translation(0, 5, 0); coneTwist = new ConeTwistConstraint(pBodyA, pBodyB, frameInA, frameInB); //coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f); //coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f); World.AddConstraint(coneTwist, true); coneTwist.DebugDrawSize = 5; // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver) RigidBody pBody = LocalCreateRigidBody(1.0f, Matrix.Identity, shape); pBody.ActivationState = ActivationState.DisableDeactivation; Vector3 pivotA = new Vector3(10.0f, 0.0f, 0.0f); btAxisA = new Vector3(0.0f, 0.0f, 1.0f); HingeConstraint pHinge = new HingeConstraint(pBody, pivotA, btAxisA); //pHinge.EnableAngularMotor(true, -1.0f, 0.165f); // use for the old solver pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver World.AddConstraint(pHinge); pHinge.DebugDrawSize = 5; // create a universal joint using generic 6DOF constraint // create two rigid bodies // static bodyA (parent) on top: pBodyA = LocalCreateRigidBody(0, Matrix.Translation(20, 4, 0), shape); pBodyA.ActivationState = ActivationState.DisableDeactivation; // dynamic bodyB (child) below it : pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(20, 0, 0), shape); pBodyB.ActivationState = ActivationState.DisableDeactivation; // add some (arbitrary) data to build constraint frames Vector3 parentAxis = new Vector3(1, 0, 0); Vector3 childAxis = new Vector3(0, 0, 1); Vector3 anchor = new Vector3(20, 2, 0); UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, anchor, parentAxis, childAxis); pUniv.SetLowerLimit(-(float)Math.PI / 4, -(float)Math.PI / 4); pUniv.SetUpperLimit((float)Math.PI / 4, (float)Math.PI / 4); // add constraint to world World.AddConstraint(pUniv, true); // draw constraint frames and limits for debugging pUniv.DebugDrawSize = 5; World.AddConstraint(pGen6DOF, true); pGen6DOF.DebugDrawSize = 5; // create a generic 6DOF constraint with springs pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 16, 0), shape); pBodyA.ActivationState = ActivationState.DisableDeactivation; pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 16, 0), shape); pBodyB.ActivationState = ActivationState.DisableDeactivation; frameInA = Matrix.Translation(10, 0, 0); frameInB = Matrix.Identity; Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, frameInA, frameInB, true) { LinearUpperLimit = new Vector3(5, 0, 0), LinearLowerLimit = new Vector3(-5, 0, 0), AngularLowerLimit = new Vector3(0, 0, -1.5f), AngularUpperLimit = new Vector3(0, 0, 1.5f), DebugDrawSize = 5 }; World.AddConstraint(pGen6DOFSpring, true); pGen6DOFSpring.EnableSpring(0, true); pGen6DOFSpring.SetStiffness(0, 39.478f); pGen6DOFSpring.SetDamping(0, 0.5f); pGen6DOFSpring.EnableSpring(5, true); pGen6DOFSpring.SetStiffness(5, 39.478f); pGen6DOFSpring.SetDamping(0, 0.3f); pGen6DOFSpring.SetEquilibriumPoint(); // create a Hinge2 joint // create two rigid bodies // static bodyA (parent) on top: pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 4, 0), shape); pBodyA.ActivationState = ActivationState.DisableDeactivation; // dynamic bodyB (child) below it : pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, 0, 0), shape); pBodyB.ActivationState = ActivationState.DisableDeactivation; // add some data to build constraint frames parentAxis = new Vector3(0, 1, 0); childAxis = new Vector3(1, 0, 0); anchor = new Vector3(-20, 0, 0); Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, anchor, parentAxis, childAxis); pHinge2.SetLowerLimit(-(float)Math.PI / 4); pHinge2.SetUpperLimit((float)Math.PI / 4); // add constraint to world World.AddConstraint(pHinge2, true); // draw constraint frames and limits for debugging pHinge2.DebugDrawSize = 5; // create a Hinge joint between two dynamic bodies // create two rigid bodies // static bodyA (parent) on top: pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), shape); pBodyA.ActivationState = ActivationState.DisableDeactivation; // dynamic bodyB: pBodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), shape); pBodyB.ActivationState = ActivationState.DisableDeactivation; // add some data to build constraint frames axisA = new Vector3(0, 1, 0); axisB = new Vector3(0, 1, 0); Vector3 pivotA2 = new Vector3(-5, 0, 0); Vector3 pivotB = new Vector3(5, 0, 0); spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, pivotA2, pivotB, axisA, axisB); spHingeDynAB.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4); // add constraint to world World.AddConstraint(spHingeDynAB, true); // draw constraint frames and limits for debugging spHingeDynAB.DebugDrawSize = 5; }
public static void DebugDrawConstraint(TypedConstraint constraint, IDebugDraw debugDraw) { bool drawFrames = (debugDraw.GetDebugMode() & DebugDrawModes.DBG_DrawConstraints) != 0; bool drawLimits = (debugDraw.GetDebugMode() & DebugDrawModes.DBG_DrawConstraintLimits) != 0; float dbgDrawSize = constraint.GetDbgDrawSize(); if (dbgDrawSize <= 0f) { return; } switch (constraint.GetConstraintType()) { case TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE: { Point2PointConstraint p2pC = constraint as Point2PointConstraint; IndexedMatrix tr = IndexedMatrix.Identity; IndexedVector3 pivot = p2pC.GetPivotInA(); pivot = p2pC.GetRigidBodyA().GetCenterOfMassTransform() * pivot; tr._origin = pivot; debugDraw.DrawTransform(ref tr, dbgDrawSize); // that ideally should draw the same frame pivot = p2pC.GetPivotInB(); pivot = p2pC.GetRigidBodyB().GetCenterOfMassTransform() * pivot; tr._origin = pivot; if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } } break; case TypedConstraintType.HINGE_CONSTRAINT_TYPE: { HingeConstraint pHinge = constraint as HingeConstraint; IndexedMatrix tr = pHinge.GetRigidBodyA().GetCenterOfMassTransform() * pHinge.GetAFrame(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } tr = pHinge.GetRigidBodyB().GetCenterOfMassTransform() * pHinge.GetBFrame(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } float minAng = pHinge.GetLowerLimit(); float maxAng = pHinge.GetUpperLimit(); if (minAng == maxAng) { break; } bool drawSect = true; if (minAng > maxAng) { minAng = 0f; maxAng = MathUtil.SIMD_2_PI; drawSect = false; } if (drawLimits) { IndexedVector3 center = tr._origin; IndexedVector3 normal = tr._basis.GetColumn(2); IndexedVector3 axis = tr._basis.GetColumn(0); IndexedVector3 zero = IndexedVector3.Zero; debugDraw.DrawArc(ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, ref zero, drawSect); } } break; case TypedConstraintType.CONETWIST_CONSTRAINT_TYPE: { ConeTwistConstraint pCT = constraint as ConeTwistConstraint; IndexedMatrix tr = pCT.GetRigidBodyA().GetCenterOfMassTransform() * pCT.GetAFrame(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } tr = pCT.GetRigidBodyB().GetCenterOfMassTransform() * pCT.GetBFrame(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } IndexedVector3 zero = IndexedVector3.Zero; if (drawLimits) { //const float length = float(5); float length = dbgDrawSize; const int nSegments = 8 * 4; float fAngleInRadians = MathUtil.SIMD_2_PI * (float)(nSegments - 1) / (float)nSegments; IndexedVector3 pPrev = pCT.GetPointForAngle(fAngleInRadians, length); pPrev = tr * pPrev; for (int i = 0; i < nSegments; i++) { fAngleInRadians = MathUtil.SIMD_2_PI * (float)i / (float)nSegments; IndexedVector3 pCur = pCT.GetPointForAngle(fAngleInRadians, length); pCur = tr * pCur; debugDraw.DrawLine(ref pPrev, ref pCur, ref zero); if (i % (nSegments / 8) == 0) { IndexedVector3 origin = tr._origin; debugDraw.DrawLine(ref origin, ref pCur, ref zero); } pPrev = pCur; } float tws = pCT.GetTwistSpan(); float twa = pCT.GetTwistAngle(); bool useFrameB = (pCT.GetRigidBodyB().GetInvMass() > 0f); if (useFrameB) { tr = pCT.GetRigidBodyB().GetCenterOfMassTransform() * pCT.GetBFrame(); } else { tr = pCT.GetRigidBodyA().GetCenterOfMassTransform() * pCT.GetAFrame(); } IndexedVector3 pivot = tr._origin; IndexedVector3 normal = tr._basis.GetColumn(0); IndexedVector3 axis = tr._basis.GetColumn(1); debugDraw.DrawArc(ref pivot, ref normal, ref axis, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, ref zero, true); } } break; case TypedConstraintType.D6_CONSTRAINT_TYPE: case TypedConstraintType.D6_SPRING_CONSTRAINT_TYPE: { Generic6DofConstraint p6DOF = constraint as Generic6DofConstraint; IndexedMatrix tr = p6DOF.GetCalculatedTransformA(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } tr = p6DOF.GetCalculatedTransformB(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } IndexedVector3 zero = IndexedVector3.Zero; if (drawLimits) { tr = p6DOF.GetCalculatedTransformA(); IndexedVector3 center = p6DOF.GetCalculatedTransformB()._origin; // up is axis 1 not 2 ? IndexedVector3 up = tr._basis.GetColumn(1); IndexedVector3 axis = tr._basis.GetColumn(0); float minTh = p6DOF.GetRotationalLimitMotor(1).m_loLimit; float maxTh = p6DOF.GetRotationalLimitMotor(1).m_hiLimit; float minPs = p6DOF.GetRotationalLimitMotor(2).m_loLimit; float maxPs = p6DOF.GetRotationalLimitMotor(2).m_hiLimit; debugDraw.DrawSpherePatch(ref center, ref up, ref axis, dbgDrawSize * .9f, minTh, maxTh, minPs, maxPs, ref zero); axis = tr._basis.GetColumn(1); float ay = p6DOF.GetAngle(1); float az = p6DOF.GetAngle(2); float cy = (float)Math.Cos(ay); float sy = (float)Math.Sin(ay); float cz = (float)Math.Cos(az); float sz = (float)Math.Sin(az); IndexedVector3 ref1 = new IndexedVector3( cy * cz * axis.X + cy * sz * axis.Y - sy * axis.Z, -sz * axis.X + cz * axis.Y, cz * sy * axis.X + sz * sy * axis.Y + cy * axis.Z); tr = p6DOF.GetCalculatedTransformB(); IndexedVector3 normal = -tr._basis.GetColumn(0); float minFi = p6DOF.GetRotationalLimitMotor(0).m_loLimit; float maxFi = p6DOF.GetRotationalLimitMotor(0).m_hiLimit; if (minFi > maxFi) { debugDraw.DrawArc(ref center, ref normal, ref ref1, dbgDrawSize, dbgDrawSize, -MathUtil.SIMD_PI, MathUtil.SIMD_PI, ref zero, false); } else if (minFi < maxFi) { debugDraw.DrawArc(ref center, ref normal, ref ref1, dbgDrawSize, dbgDrawSize, minFi, maxFi, ref zero, false); } tr = p6DOF.GetCalculatedTransformA(); IndexedVector3 bbMin = p6DOF.GetTranslationalLimitMotor().m_lowerLimit; IndexedVector3 bbMax = p6DOF.GetTranslationalLimitMotor().m_upperLimit; debugDraw.DrawBox(ref bbMin, ref bbMax, ref tr, ref zero); } } break; case TypedConstraintType.SLIDER_CONSTRAINT_TYPE: { SliderConstraint pSlider = constraint as SliderConstraint; IndexedMatrix tr = pSlider.GetCalculatedTransformA(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } tr = pSlider.GetCalculatedTransformB(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } IndexedVector3 zero = IndexedVector3.Zero; if (drawLimits) { IndexedMatrix tr2 = pSlider.GetCalculatedTransformA(); IndexedVector3 li_min = tr2 * new IndexedVector3(pSlider.GetLowerLinLimit(), 0f, 0f); IndexedVector3 li_max = tr2 * new IndexedVector3(pSlider.GetUpperLinLimit(), 0f, 0f); debugDraw.DrawLine(ref li_min, ref li_max, ref zero); IndexedVector3 normal = tr._basis.GetColumn(0); IndexedVector3 axis = tr._basis.GetColumn(1); float a_min = pSlider.GetLowerAngLimit(); float a_max = pSlider.GetUpperAngLimit(); IndexedVector3 center = pSlider.GetCalculatedTransformB()._origin; debugDraw.DrawArc(ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, a_min, a_max, ref zero, true); } } break; default: break; } return; }