public virtual void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex) { //(void)partId; //(void)triangleIndex; IndexedVector3 wv0, wv1, wv2; wv0 = m_worldTrans * triangle[0]; wv1 = m_worldTrans * triangle[1]; wv2 = m_worldTrans * triangle[2]; if ((int)(m_debugDrawer.GetDebugMode() & DebugDrawModes.DBG_DrawNormals) != 0) { IndexedVector3 center = (wv0 + wv1 + wv2) * (1f / 3f); IndexedVector3 normal = (wv1 - wv0).Cross(wv2 - wv0); normal.Normalize(); IndexedVector3 normalColor = new IndexedVector3(1, 1, 0); //m_debugDrawer.DrawLine(center, center + normal, normalColor); } m_debugDrawer.DrawLine(ref wv0, ref wv1, ref m_color); m_debugDrawer.DrawLine(ref wv1, ref wv2, ref m_color); m_debugDrawer.DrawLine(ref wv2, ref wv0, ref m_color); }
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 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 override int StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep) { StartProfiling(timeStep); BulletGlobals.StartProfile("stepSimulation"); int numSimulationSubSteps = 0; if (maxSubSteps != 0) { //fixed timestep with interpolation m_localTime += timeStep; if (m_localTime >= fixedTimeStep) { numSimulationSubSteps = (int)(m_localTime / fixedTimeStep); m_localTime -= numSimulationSubSteps * fixedTimeStep; } } else { //variable timestep fixedTimeStep = timeStep; m_localTime = timeStep; if (MathUtil.FuzzyZero(timeStep)) { numSimulationSubSteps = 0; maxSubSteps = 0; } else { numSimulationSubSteps = 1; maxSubSteps = 1; } } //process some debugging flags if (GetDebugDrawer() != null) { IDebugDraw debugDrawer = GetDebugDrawer(); BulletGlobals.gDisableDeactivation = ((debugDrawer.GetDebugMode() & DebugDrawModes.DBG_NoDeactivation) != 0); } if (numSimulationSubSteps != 0) { //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld) { BulletGlobals.g_streamWriter.WriteLine(String.Format("Stepsimulation numClamped[{0}] timestep[{1:0.00000}]", clampedSimulationSteps, fixedTimeStep)); } #endif SaveKinematicState(fixedTimeStep * clampedSimulationSteps); ApplyGravity(); for (int i = 0; i < clampedSimulationSteps; i++) { InternalSingleStepSimulation(fixedTimeStep); SynchronizeMotionStates(); } } else { SynchronizeMotionStates(); } ClearForces(); if (m_profileManager != null) { m_profileManager.Increment_Frame_Counter(); } return(numSimulationSubSteps); }
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; IConstraintTypeDrawer constraintTypeDrawer; if (!ConstraintDrawers.TryGetValue(constraint.GetConstraintType(), out constraintTypeDrawer)) return; constraintTypeDrawer.DrawFrames = drawFrames; constraintTypeDrawer.DrawLimits = drawLimits; constraintTypeDrawer.DrawSize = dbgDrawSize; constraintTypeDrawer.Draw(constraint, debugDraw); }