public unsafe static void SetPivotB(this Point2PointConstraint obj, ref OpenTK.Vector3 pivotB) { fixed(OpenTK.Vector3 *pivotBPtr = &pivotB) { obj.PivotInB = *(BulletSharp.Math.Vector3 *)pivotBPtr; } }
//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 = targetRigidBodyA.GetRigidBody(); if (rba == null) { Debug.LogError("Constraint could not get bullet RigidBody from target rigid body A"); return false; } if (constraintType == ConstraintType.constrainToAnotherBody) { RigidBody rbb = targetRigidBodyB.GetRigidBody(); if (rbb == null) { Debug.LogError("Constraint could not get bullet RigidBody from target rigid body B"); return false; } constraintPtr = new Point2PointConstraint(rba, rbb, pivotInA.ToBullet(), pivotInB.ToBullet()); } else { constraintPtr = new Point2PointConstraint(rba,pivotInA.ToBullet()); } return true; }
public unsafe static void GetPivotInB(this Point2PointConstraint obj, out OpenTK.Vector3 value) { fixed(OpenTK.Vector3 *valuePtr = &value) { *(BulletSharp.Math.Vector3 *)valuePtr = obj.PivotInB; } }
//public void Test() //{ // BulletSharp.Point2PointConstraint pt; // /BulletSharp.Generic6DofConstraint sdof = new Generic6DofConstraint( // pt.Setting. //sdof.sett //} public override TypedConstraint GetConstraint(RigidBody body) { Point2PointConstraint cst = new Point2PointConstraint(body, this.Pivot); cst.Setting.Damping = this.Damping; cst.Setting.ImpulseClamp = this.ImpulseClamp; cst.Setting.Tau = this.Tau; return cst; }
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\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.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 }; 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(); } }
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(); LibraryManager.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; CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(ref rayFrom, ref rayTo); _world.RayTest(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.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; } } } }
protected override void OnInitializePhysics() { // collision configuration contains default setup for memory, collision setup CollisionConf = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConf); Broadphase = new DbvtBroadphase(); World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf); World.Gravity = new Vector3(0, -10, 0); GImpactCollisionAlgorithm.RegisterAlgorithm(Dispatcher); string bulletFile; string[] args = Environment.GetCommandLineArgs(); if (args.Length == 1) { bulletFile = "testFile.bullet"; } else { bulletFile = args[1]; } fileLoader = new CustomBulletWorldImporter(World); if (!fileLoader.LoadFile(bulletFile)) { CollisionShape groundShape = new BoxShape(50); CollisionShapes.Add(groundShape); RigidBody ground = LocalCreateRigidBody(0, Matrix.Translation(0, -50, 0), groundShape); ground.UserObject = "Ground"; // create a few dynamic rigidbodies float mass = 1.0f; Vector3[] positions = new Vector3[2] { new Vector3(0.1f, 0.2f, 0.3f), new Vector3(0.4f, 0.5f, 0.6f) }; float[] radi = new float[2] { 0.3f, 0.4f }; CollisionShape colShape = new MultiSphereShape(positions, radi); //CollisionShape colShape = new CapsuleShapeZ(1, 1); //CollisionShape colShape = new CylinderShapeZ(1, 1, 1); //CollisionShape colShape = new BoxShape(1); //CollisionShape colShape = new SphereShape(1); CollisionShapes.Add(colShape); Vector3 localInertia = colShape.CalculateLocalInertia(mass); float start_x = StartPosX - ArraySizeX / 2; float start_y = StartPosY; float start_z = StartPosZ - ArraySizeZ / 2; int k, i, j; for (k = 0; k < ArraySizeY; k++) { for (i = 0; i < ArraySizeX; i++) { for (j = 0; j < ArraySizeZ; j++) { Matrix startTransform = Matrix.Translation( 2 * i + start_x, 2 * k + start_y, 2 * j + start_z ); // using motionstate is recommended, it provides interpolation capabilities // and only synchronizes 'active' objects DefaultMotionState myMotionState = new DefaultMotionState(startTransform); RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia); RigidBody body = new RigidBody(rbInfo); rbInfo.Dispose(); // make it drop from a height body.Translate(new Vector3(0, 20, 0)); World.AddRigidBody(body); } } } DefaultSerializer serializer = new DefaultSerializer(); serializer.RegisterNameForObject(ground, "GroundName"); for (i = 0; i < CollisionShapes.Count; i++) serializer.RegisterNameForObject(CollisionShapes[i], "name" + i.ToString()); Point2PointConstraint p2p = new Point2PointConstraint((RigidBody)World.CollisionObjectArray[2], new Vector3(0, 1, 0)); World.AddConstraint(p2p); serializer.RegisterNameForObject(p2p, "constraintje"); World.Serialize(serializer); BulletSharp.DataStream data = serializer.LockBuffer(); byte[] dataBytes = new byte[data.Length]; data.Read(dataBytes, 0, dataBytes.Length); FileStream file = new FileStream("testFile.bullet", FileMode.Create); file.Write(dataBytes, 0, dataBytes.Length); file.Close(); } }
protected override void OnInitializePhysics() { SetupEmptyDynamicsWorld(); IsDebugDrawEnabled = true; 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 Point2PointConstraint CreatePoint2PointConstraint(RigidBody rbA, ref Vector3 pivotInA) { Point2PointConstraint constraint = new Point2PointConstraint(rbA, pivotInA); _allocatedConstraints.Add(constraint); return constraint; }
protected virtual void OnHandleInput() { if (Input.KeysPressed.Count != 0) { Keys key = Input.KeysPressed[0]; switch (key) { case Keys.Escape: case Keys.Q: Quit(); return; case Keys.F3: IsDebugDrawEnabled = !IsDebugDrawEnabled; break; case Keys.F11: ToggleFullScreen(); break; case Keys.Space: PhysicsContext.ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView)); break; } } if (Input.MousePressed != MouseButtonFlags.None) { Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView); if (Input.MousePressed == MouseButtonFlags.RightDown) { if (PhysicsContext.World != null) { Vector3 rayFrom = Freelook.Eye; CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.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.MousePressed == MouseButtonFlags.RightUp) { if (pickConstraint != null && PhysicsContext.World != null) { PhysicsContext.World.RemoveConstraint(pickConstraint); pickConstraint.Dispose(); pickConstraint = null; pickedBody.ForceActivationState(ActivationState.ActiveTag); pickedBody.DeactivationTime = 0; pickedBody = null; } } } // Mouse movement if (Input.MouseDown == MouseButtonFlags.RightDown) { 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; } } } }
public static OpenTK.Vector3 GetPivotInB(this Point2PointConstraint obj) { OpenTK.Vector3 value; GetPivotInB(obj, out value); return(value); }
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; DefaultSerializer serializer = new DefaultSerializer(maxSerializeBufferSize); PhysicsContext.World.Serialize(serializer); byte[] dataBytes = new byte[serializer.CurrentBufferSize]; System.Runtime.InteropServices.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.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; ClosestRayResultCallback 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 }; 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); */ } 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 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(); } }
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; }