protected virtual void CalculateSimulationIslands() { BulletGlobals.StartProfile("calculateSimulationIslands"); GetSimulationIslandManager().UpdateActivationState(GetCollisionWorld(), GetCollisionWorld().GetDispatcher()); { int length = m_constraints.Count; for (int i = 0; i < length; ++i) { TypedConstraint constraint = m_constraints[i]; RigidBody colObj0 = constraint.GetRigidBodyA(); RigidBody colObj1 = constraint.GetRigidBodyB(); if (((colObj0 != null) && (!colObj0.IsStaticOrKinematicObject())) && ((colObj1 != null) && (!colObj1.IsStaticOrKinematicObject()))) { if (colObj0.IsActive() || colObj1.IsActive()) { GetSimulationIslandManager().GetUnionFind().Unite((colObj0).GetIslandTag(), (colObj1).GetIslandTag()); } } } } //Store the island id in each body GetSimulationIslandManager().StoreIslandActivationState(GetCollisionWorld()); BulletGlobals.StopProfile(); }
public override void DebugDrawWorld() { BulletGlobals.StartProfile("debugDrawWorld"); base.DebugDrawWorld(); if (GetDebugDrawer() != null) { DebugDrawModes mode = GetDebugDrawer().GetDebugMode(); if ((mode & (DebugDrawModes.DBG_DrawConstraints | DebugDrawModes.DBG_DrawConstraintLimits)) != 0) { for (int i = GetNumConstraints() - 1; i >= 0; i--) { TypedConstraint constraint = GetConstraint(i); DrawHelper.DebugDrawConstraint(constraint, GetDebugDrawer()); } } if (mode != 0) { int actionsCount = m_actions.Count; for (int i = 0; i < actionsCount; ++i) { m_actions[i].DebugDraw(m_debugDrawer); } } } BulletGlobals.StopProfile(); }
public static int GetConstraintIslandId(TypedConstraint lhs) { int islandId; CollisionObject rcolObj0 = lhs.GetRigidBodyA(); CollisionObject rcolObj1 = lhs.GetRigidBodyB(); islandId = rcolObj0.GetIslandTag() >= 0 ? rcolObj0.GetIslandTag() : rcolObj1.GetIslandTag(); return(islandId); }
public void AddConstraintRef(TypedConstraint c) { if (!m_constraintRefs.Contains(c)) { m_constraintRefs.Add(c); } m_checkCollideWith = true; }
public static void PrintInfo1(StreamWriter writer, TypedConstraint constraint, ConstraintInfo1 info) { if (writer != null) { writer.WriteLine("getInfo1 [{0}] [{1}] [{2}] [{3}]", constraint.m_userConstraintId, constraint.GetObjectType(), (string)constraint.GetRigidBodyA().GetUserPointer(), (string)constraint.GetRigidBodyB().GetUserPointer()); MathUtil.PrintMatrix(writer, "rBA cmot", constraint.GetRigidBodyA().GetCenterOfMassTransform()); MathUtil.PrintMatrix(writer, "rBB cmot", constraint.GetRigidBodyB().GetCenterOfMassTransform()); MathUtil.PrintMatrix(writer, "rBA inv tensor", constraint.GetRigidBodyA().GetInvInertiaTensorWorld()); MathUtil.PrintMatrix(writer, "rBB inv tensor", constraint.GetRigidBodyB().GetInvInertiaTensorWorld()); writer.WriteLine(String.Format("NumRows [{0}] Nub[{1}]", info.m_numConstraintRows, info.nub)); } }
public override void AddConstraint(TypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies) { //if (constraint is ConeTwistConstraint) //if (constraint is HingeConstraint) //{ // return; //} m_constraints.Add(constraint); if (disableCollisionsBetweenLinkedBodies) { constraint.GetRigidBodyA().AddConstraintRef(constraint); constraint.GetRigidBodyB().AddConstraintRef(constraint); } }
public static void PrintInfo2(StreamWriter writer, TypedConstraint constraint, ConstraintInfo2 info2) { if (writer != null) { writer.WriteLine(String.Format("getInfo2 [{0}] [{1}] [{2}] [{3}]", constraint.m_userConstraintId, constraint.GetObjectType(), (string)constraint.GetRigidBodyA().GetUserPointer(), (string)constraint.GetRigidBodyB().GetUserPointer())); writer.WriteLine(String.Format("numRows [{0}] fps[{1:0.00000000}] erp[{2:0.00000000}] findex[{3}] numIter[{4}]", info2.m_numRows, info2.fps, info2.erp, info2.findex, info2.m_numIterations)); for (int i = 0; i < info2.m_numRows; ++i) { writer.WriteLine(String.Format("TypedConstraint[{0}]", i)); writer.WriteLine("ContactNormal"); MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_contactNormal); writer.WriteLine("rel1pos1CrossNormal"); MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_relpos1CrossNormal); writer.WriteLine("rel1pos2CrossNormal"); MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_relpos2CrossNormal); } } }
public override bool CheckCollideWithOverride(CollisionObject co) { RigidBody otherRb = RigidBody.Upcast(co); if (otherRb == null) { return(true); } for (int i = 0; i < m_constraintRefs.Count; ++i) { TypedConstraint c = m_constraintRefs[i]; if (c.IsEnabled()) { if (c.GetRigidBodyA() == otherRb || c.GetRigidBodyB() == otherRb) { return(false); } } } return(true); }
public override void RemoveConstraint(TypedConstraint constraint) { m_constraints.Remove(constraint); constraint.GetRigidBodyA().RemoveConstraintRef(constraint); constraint.GetRigidBodyB().RemoveConstraintRef(constraint); }
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; }
public void RemoveConstraintRef(TypedConstraint c) { m_constraintRefs.Remove(c); m_checkCollideWith = m_constraintRefs.Count > 0; }
public virtual void AddConstraint(TypedConstraint constraint) { AddConstraint(constraint, false); }
//---------------------------------------------------------------------------------------------- public DemoApplication() { m_dynamicsWorld = null; m_pickConstraint = null; m_shootBoxShape = null; m_cameraDistance = 30f; m_pitch =(20f/360f)*MathUtil.SIMD_2_PI; m_yaw = 0f; m_cameraPosition = IndexedVector3.Zero; m_cameraTargetPosition = IndexedVector3.Zero; m_scaleBottom = 0.5f; m_scaleFactor = 2f; m_cameraUp = new IndexedVector3(0, 1, 0); m_forwardAxis = 2; m_glutScreenWidth = 0; m_glutScreenHeight = 0; m_ShootBoxInitialSpeed = 40f; m_stepping = true; m_singleStep = false; m_idle = false; m_enableshadows = true; m_lightPosition = new IndexedVector3(5, 5, 5); //m_lightDirection = IndexedVector3.Down; m_lightDirection = new IndexedVector3(.5f, -.5f, .5f); m_lightDirection.Normalize(); //#ifndef BT_NO_PROFILE // m_profileIterator = CProfileManager::Get_Iterator(); //#endif //BT_NO_PROFILE Content.RootDirectory = "Content"; m_graphics = new GraphicsDeviceManager(this); m_graphics.PreferredBackBufferWidth = 800; m_graphics.PreferredBackBufferHeight = 600; SetSize(m_graphics.PreferredBackBufferWidth,m_graphics.PreferredBackBufferHeight); m_nearClip = 1f; m_farClip = 1000f; m_aspect = m_glutScreenWidth / m_glutScreenHeight; m_perspective = IndexedMatrix.CreatePerspectiveFieldOfView(MathHelper.ToRadians(40.0f), m_aspect, m_nearClip, m_farClip); }
public virtual void AddConstraint(TypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies) { //(void)constraint; //(void)disableCollisionsBetweenLinkedBodies; }
public BulletConstraintXNA(TypedConstraint xx) : base() { constrain = xx; }
public virtual void RemoveConstraint(TypedConstraint constraint) { //(void)constraint; }
public override void Clear() { constrain = null; }
//---------------------------------------------------------------------------------------------- public virtual void MouseFunc(ref MouseState oldMouseState, ref MouseState newMouseState) { IndexedVector3 rayTo = GetRayTo(newMouseState.X, newMouseState.Y); if (WasReleased(ref oldMouseState,ref newMouseState,2)) { ShootBox(rayTo); } else if (WasReleased(ref oldMouseState,ref newMouseState,1)) { //apply an impulse if (m_dynamicsWorld != null) { ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(new IndexedVector3(m_cameraPosition), rayTo); IndexedVector3 ivPos = new IndexedVector3(m_cameraPosition); IndexedVector3 ivTo = new IndexedVector3(rayTo); m_dynamicsWorld.RayTest(ref ivPos,ref ivTo, rayCallback); if (rayCallback.HasHit()) { RigidBody body = RigidBody.Upcast(rayCallback.m_collisionObject); if (body != null) { body.SetActivationState(ActivationState.ACTIVE_TAG); IndexedVector3 impulse = rayTo; impulse.Normalize(); float impulseStrength = 10f; impulse *= impulseStrength; IndexedVector3 relPos = rayCallback.m_hitPointWorld - body.GetCenterOfMassPosition(); body.ApplyImpulse(ref impulse, ref relPos); } } } } else if (newMouseState.LeftButton == ButtonState.Pressed) { //add a point to point constraint for picking if (m_dynamicsWorld != null) { IndexedVector3 rayFrom; if (m_ortho) { rayFrom = rayTo; rayFrom.Z = -100.0f; } else { rayFrom = new IndexedVector3(m_cameraPosition); } ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo); IndexedVector3 ivPos = new IndexedVector3(m_cameraPosition); IndexedVector3 ivTo = new IndexedVector3(rayTo); m_dynamicsWorld.RayTest(ref ivPos, ref ivTo, rayCallback); if (rayCallback.HasHit()) { RigidBody body = RigidBody.Upcast(rayCallback.m_collisionObject); if (body != null) { //other exclusions? if (!(body.IsStaticObject() || body.IsKinematicObject())) { pickedBody = body; pickedBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedVector3 pickPos = rayCallback.m_hitPointWorld; IndexedVector3 localPivot = body.GetCenterOfMassTransform().Inverse() * pickPos; if (use6Dof) { IndexedMatrix tr = IndexedMatrix.Identity; tr._origin = localPivot; Generic6DofConstraint dof6 = new Generic6DofConstraint(body, ref tr, false); dof6.SetLinearLowerLimit(new IndexedVector3(0, 0, 0)); dof6.SetLinearUpperLimit(new IndexedVector3(0, 0, 0)); dof6.SetAngularLowerLimit(new IndexedVector3(0, 0, 0)); dof6.SetAngularUpperLimit(new IndexedVector3(0, 0, 0)); m_dynamicsWorld.AddConstraint(dof6); m_pickConstraint = dof6; dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_CFM, 0.8f, 0); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_CFM, 0.8f, 1); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_CFM, 0.8f, 2); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_CFM, 0.8f, 3); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_CFM, 0.8f, 4); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_CFM, 0.8f, 5); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_ERP, 0.1f, 0); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_ERP, 0.1f, 1); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_ERP, 0.1f, 2); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_ERP, 0.1f, 3); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_ERP, 0.1f, 4); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_ERP, 0.1f, 5); } else { Point2PointConstraint p2p = new Point2PointConstraint(body, ref localPivot); m_dynamicsWorld.AddConstraint(p2p, false); p2p.m_setting.m_impulseClamp = mousePickClamping; p2p.m_setting.m_tau = 0.001f; if (m_pickConstraint != null) { int ibreak = 0; } m_pickConstraint = p2p; } //save mouse position for dragging gOldPickingPos = rayTo; gHitPos = pickPos; gOldPickingDist = (pickPos - rayFrom).Length(); //very weak constraint for picking } } } } } else if (WasReleased(ref oldMouseState,ref newMouseState,0)) { if (m_pickConstraint != null && m_dynamicsWorld != null) { m_dynamicsWorld.RemoveConstraint(m_pickConstraint); m_pickConstraint = null; //printf("removed constraint %i",gPickingConstraintId); m_pickConstraint = null; pickedBody.ForceActivationState(ActivationState.ACTIVE_TAG); pickedBody.SetDeactivationTime(0f); pickedBody = null; } } }