public void Evaluate(int SpreadMax) { IConstraintContainer inputWorld = this.contraintContainer[0]; if (inputWorld != null) { this.persister.UpdateWorld(inputWorld); for (int i = 0; i < SpreadMax; i++) { if (FCreate[i]) { RigidBodyPair pair = FPairs[i]; if (pair.body1 != null && pair.body2 != null) { Point2PointConstraint cst = new Point2PointConstraint(pair.body1, pair.body2, this.FPivot1[i].ToBulletVector(), this.FPivot2[i].ToBulletVector()); cst.Setting.Damping = this.FDamping[i]; cst.Setting.ImpulseClamp = this.FImpulseClamp[i]; cst.Setting.Tau = this.FTau[i]; this.persister.Append(cst); } } } this.persister.Flush(); } else { this.constraintsOutput.SliceCount = 0; } }
//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() { WorldController world = GetWorld(); 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("BallSocketConstraint 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 A"); return(false); } if (m_constraintType == ConstraintType.constrainToAnotherBody) { if (m_otherRigidBody == null) { Debug.LogError("Other rigid body was not set"); return(false); } if (!m_otherRigidBody.isInWorld) { world.AddRigidBody(m_otherRigidBody); } RigidBody rbb = (RigidBody)m_otherRigidBody.GetCollisionObject(); if (rbb == null) { Debug.LogError("Constraint could not get bullet RigidBody from target rigid body B"); return(false); } Vector3 pivotInOther = m_otherRigidBody.transform.InverseTransformPoint(targetRigidBodyA.transform.TransformPoint(m_localConstraintPoint)); m_constraintPtr = new Point2PointConstraint(rbb, rba, pivotInOther.ToBullet(), m_localConstraintPoint.ToBullet()); } else { m_constraintPtr = new Point2PointConstraint(rba, m_localConstraintPoint.ToBullet()); } m_constraintPtr.Userobject = this; m_constraintPtr.BreakingImpulseThreshold = m_breakingImpulseThreshold; m_constraintPtr.DebugDrawSize = m_debugDrawSize; m_constraintPtr.OverrideNumSolverIterations = m_overrideNumSolverIterations; return(true); }
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; } } }
void PickBody() { DropBody(); PickedEntity = RayCastEntity(out Vector3 PickPoint, out Vector3 Normal); if (PickedEntity != null) { DbgDraw.DrawLine(FireOrigin, PickPoint, Time: 5000); DbgDraw.DrawCircle(PickPoint + Normal * 0.1f, Normal, FishGfx.Color.Red, 5, 8, Time: 5000); PickDist = Vector3.Distance(FireOrigin, PickPoint); GlobalPickPoint = PickPoint; Matrix4x4.Invert(PickedEntity.RigidBody.CenterOfMassTransform, out Matrix4x4 InvCenterOfMass); LocalPickPoint = PickPoint = Vector3.Transform(PickPoint, InvCenterOfMass); PickedEntity.Unfreeze(); PickedEntity.DisableSleep(); PointConst = new Point2PointConstraint(PickedEntity.RigidBody, PickPoint); PointConst.Setting.ImpulseClamp = 10000; PointConst.Setting.Tau = 0.01f; PointConst.Setting.Damping = 0.99f; Map.World.AddConstraint(PointConst); OldAngularDamping = PickedEntity.RigidBody.AngularDamping; OldLinearDamping = PickedEntity.RigidBody.LinearDamping; PickedEntity.RigidBody.SetDamping(OldLinearDamping, 0.99f); IsPickingBody = true; } }
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 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 void Evaluate(int SpreadMax) { for (int i = 0; i < SpreadMax; i++) { if (this.FApply[i] && this.FConstraint[i] != null) { Point2PointConstraint cst = this.FConstraint[i]; cst.PivotInA = this.FPivot1[i].ToBulletVector(); } } }
//Point2point public IPoint2PointConstraintImp AddPoint2PointConstraint(IRigidBodyImp rigidBodyA, float3 pivotInA) { var rigidBodyAImp = (RigidBodyImp)rigidBodyA; var btRigidBodyA = rigidBodyAImp._rbi; var btP2PConstraint = new Point2PointConstraint(btRigidBodyA, new Vector3(pivotInA.x, pivotInA.y, pivotInA.z)); BtWorld.AddConstraint(btP2PConstraint); var retval = new Point2PointConstraintImp(); retval._p2pci = btP2PConstraint; btP2PConstraint.UserObject = retval; return(retval); }
void DropBody() { if (!IsPickingBody) { return; } Map.World.RemoveConstraint(PointConst); PointConst.Dispose(); PointConst = null; PickedEntity.Unfreeze(); PickedEntity.RigidBody.SetDamping(OldLinearDamping, OldAngularDamping); PickedEntity = null; IsPickingBody = false; }
//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 A"); 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 B"); return(false); } constraintPtr = new Point2PointConstraint(rba, rbb, pivotInA.ToBullet(), pivotInB.ToBullet()); } else { constraintPtr = new Point2PointConstraint(rba, pivotInA.ToBullet()); } constraintPtr.Userobject = this; return(true); }
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 IPoint2PointConstraintImp AddPoint2PointConstraint(IRigidBodyImp rigidBodyA, IRigidBodyImp rigidBodyB, float3 pivotInA,float3 pivotInB) { var rigidBodyAImp = (RigidBodyImp) rigidBodyA; var btRigidBodyA = rigidBodyAImp._rbi; var rigidBodyBImp = (RigidBodyImp) rigidBodyB; var btRigidBodyB = rigidBodyBImp._rbi; var btP2PConstraint = new Point2PointConstraint(btRigidBodyA, btRigidBodyB, new Vector3(pivotInA.x, pivotInA.y, pivotInA.z), new Vector3(pivotInB.x, pivotInB.y, pivotInB.z)); BtWorld.AddConstraint(btP2PConstraint); var retval = new Point2PointConstraintImp(); retval._p2pci = btP2PConstraint; btP2PConstraint.UserObject = retval; return retval; }
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(); } }
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; }
//Initialize and register the constraint protected override void InitializeConstraint() { _constraint = new Point2PointConstraint(_rigidbody.BodyInstance, _connectedBody.BodyInstance, _localPivot.ToBullet(), _connectedPivot.ToBullet()); }
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 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(); } }
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 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 SerializeDemoSimulation() { CollisionConfiguration = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConfiguration); Broadphase = new DbvtBroadphase(); World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConfiguration); 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)) { var groundShape = new BoxShape(50); _collisionShapes.Add(groundShape); RigidBody ground = PhysicsHelper.CreateStaticBody(Matrix.Translation(0, -50, 0), groundShape, World); ground.UserObject = "Ground"; // create a few dynamic rigidbodies float mass = 1.0f; Vector3[] positions = new[] { 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 }; var colShape = new MultiSphereShape(positions, radi); //var colShape = new CapsuleShapeZ(1, 1); //var colShape = new CylinderShapeZ(1, 1, 1); //var colShape = new BoxShape(1); //var colShape = new SphereShape(1); _collisionShapes.Add(colShape); Vector3 localInertia = colShape.CalculateLocalInertia(mass); float startX = StartPosX - NumObjectsX / 2; float startY = StartPosY; float startZ = StartPosZ - NumObjectsZ / 2; for (int y = 0; y < NumObjectsY; y++) { for (int x = 0; x < NumObjectsX; x++) { for (int z = 0; z < NumObjectsZ; z++) { Matrix startTransform = Matrix.Translation( 2 * x + startX, 2 * y + startY, 2 * z + startZ ); // 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); } } } using (var serializer = new DefaultSerializer()) { serializer.RegisterNameForObject(ground, "GroundName"); for (int i = 0; i < _collisionShapes.Count; i++) { serializer.RegisterNameForObject(_collisionShapes[i], "name" + i.ToString()); } var p2p = new Point2PointConstraint((RigidBody)World.CollisionObjectArray[2], new Vector3(0, 1, 0)); World.AddConstraint(p2p); serializer.RegisterNameForObject(p2p, "constraintje"); World.Serialize(serializer); byte[] dataBytes = new byte[serializer.CurrentBufferSize]; Marshal.Copy(serializer.BufferPointer, dataBytes, 0, dataBytes.Length); using (var file = new FileStream("testFile.bullet", FileMode.Create)) { file.Write(dataBytes, 0, dataBytes.Length); } } } }
//------------------------------------------------------ // 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 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; } } } }
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(); } }
public override void InitializeDemo() { //string filename = @"E:\users\man\bullet\xna-constraint-output.txt"; //FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read); //BulletGlobals.g_streamWriter = new StreamWriter(filestream); //maxiterations = 100; 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 with a breaking threshold { trans = IndexedMatrix.Identity; trans._origin = new IndexedVector3(1, 30, -5); LocalCreateRigidBody(mass, trans, shape); trans._origin = new IndexedVector3(0, 0, -5); RigidBody body0 = LocalCreateRigidBody(mass, trans, shape); trans._origin = new IndexedVector3(2 * CUBE_HALF_EXTENTS, 20, 0); mass = 1.0f; RigidBody body1 = null;//localCreateRigidBody( mass,trans,shape); IndexedVector3 pivotInA = new IndexedVector3(CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS, 0); TypedConstraint p2p = new Point2PointConstraint(body0, ref pivotInA); m_dynamicsWorld.AddConstraint(p2p); p2p.SetBreakingImpulseThreshold(10.2f); p2p.SetDbgDrawSize(5.0f); } #endif #if true //point to point constraint (ball socket) //SEEMS OK { //trans = IndexedMatrix.Identity; 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; #if P2P TypedConstraint p2p = new Point2PointConstraint(body0, ref pivotInA); //btTypedConstraint* p2p = new btPoint2PointConstraint(*body0,*body1,pivotInA,pivotInB); //btTypedConstraint* hinge = new btHingeConstraint(*body0,*body1,pivotInA,pivotInB,axisInA,axisInB); m_dynamicsWorld.AddConstraint(p2p); p2p.SetDbgDrawSize(5.0f); #else 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 } #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(new IndexedQuaternion(sliderAxis, 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.SetAngularLowerLimit(new IndexedVector3(-MathUtil.SIMD_PI, 0, 0)); spSlider6Dof.SetAngularUpperLimit(new IndexedVector3(1.5f, 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 * 0.6f, MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_PI * 0.8f, 0.5f); // 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 #if true { // 6DOF connected to the world, with motor IndexedMatrix tr = IndexedMatrix.CreateTranslation(10, -15, 0); RigidBody pBody = LocalCreateRigidBody(1.0f, ref tr, shape); pBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedMatrix frameB = IndexedMatrix.Identity; Generic6DofConstraint pGen6Dof = new Generic6DofConstraint(pBody, ref frameB, false); m_dynamicsWorld.AddConstraint(pGen6Dof); pGen6Dof.SetDbgDrawSize(5.0f); pGen6Dof.SetAngularLowerLimit(new IndexedVector3(0, 0, 0)); pGen6Dof.SetAngularUpperLimit(new IndexedVector3(0, 0, 0)); pGen6Dof.SetLinearLowerLimit(new IndexedVector3(-10.0f, 0, 0)); pGen6Dof.SetLinearUpperLimit(new IndexedVector3(10.0f, 0, 0)); pGen6Dof.GetTranslationalLimitMotor().m_enableMotor[0] = true; pGen6Dof.GetTranslationalLimitMotor().m_targetVelocity[0] = 5.0f; pGen6Dof.GetTranslationalLimitMotor().m_maxMotorForce[0] = 0.1f; } #endif }
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; }
/// <summary> /// Called when the game has determined that game logic needs to be processed. /// </summary> /// <param name="gameTime">Time passed since the last call to this function.</param> protected override void Update(GameTime gameTime) { MouseState mouseState = Mouse.GetState(); Vector3 rayTo = getRayTo(mouseState.X, mouseState.Y); if (mouseState.LeftButton == ButtonState.Pressed && _prevMouseState.LeftButton == ButtonState.Released) { shootBox(rayTo); } if (mouseState.MiddleButton == ButtonState.Pressed && _prevMouseState.MiddleButton == ButtonState.Released) { if (_world != null) { CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(_camera.Position, rayTo); _world.RayTest(_camera.Position, rayTo, rayCallback); if (rayCallback.HasHit) { RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject); if (body != null) { //other exclusions? if (!(body.IsStaticObject || body.IsKinematicObject)) { _pickedBody = body; _pickedBody.ActivationState = ActivationState.DisableDeactivation; Vector3 pickPos = rayCallback.HitPointWorld; Vector3 localPivot = Vector3.Transform(pickPos, XnaDevRu.BulletX.MathHelper.InvertMatrix(body.CenterOfMassTransform)); Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot); _world.AddConstraint(p2p); _pickConstraint = p2p; //save mouse position for dragging _oldPickingPos = rayTo; Vector3 eyePos = new Vector3(_camera.Position.X, _camera.Position.Y, _camera.Position.Z); _oldPickingDist = (eyePos - pickPos).Length(); //very weak constraint for picking p2p.Settings.Tau = 1.1f; } } } } } else if (mouseState.MiddleButton == ButtonState.Released && _prevMouseState.MiddleButton == ButtonState.Pressed) { if (_pickConstraint != null && _world != null) { _world.RemoveConstraint(_pickConstraint); _pickConstraint = null; _pickedBody.ForceActivationState(ActivationState.Active); _pickedBody.DeactivationTime = 0f; _pickedBody = null; } } if (_pickConstraint != null) { //move the constraint pivot Point2PointConstraint p2p = _pickConstraint as Point2PointConstraint; if (p2p != null) { //keep it at the same picking distance Vector3 dir = rayTo - _camera.Position; dir.Normalize(); dir *= _oldPickingDist; Vector3 newPos = _camera.Position + dir; p2p.PivotInB = newPos; } } _prevMouseState = mouseState; if (Keyboard.GetState().IsKeyDown(Keys.Space)) { //world.stepSimulation(1.0f/60.0f,0); int numObjects = _world.CollisionObjectsCount; for (int i = 0; i < numObjects; i++) { CollisionObject colObj = _world.CollisionObjects[i]; RigidBody body = RigidBody.Upcast(colObj); if (body != null) { if (body.MotionState != null) { DefaultMotionState myMotionState = (DefaultMotionState)body.MotionState; myMotionState.GraphicsWorldTransform = myMotionState.StartWorldTransform; colObj.WorldTransform = myMotionState.GraphicsWorldTransform; colObj.InterpolationWorldTransform = myMotionState.StartWorldTransform; colObj.Activate(); } //removed cached contact points _world.Broadphase.CleanProxyFromPairs(colObj.Broadphase); if (body != null && !body.IsStaticObject) { RigidBody.Upcast(colObj).LinearVelocity = new Vector3(0, 0, 0); RigidBody.Upcast(colObj).AngularVelocity = new Vector3(0, 0, 0); } } } } else if (Keyboard.GetState().IsKeyDown(Keys.Escape) || GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed) { Exit(); } else { //world.stepSimulation(1.0f / 60.0f, 1); } base.Update(gameTime); }