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 void AddConstraintRef(TypedConstraint c)
{
if (!m_constraintRefs.Contains(c))
{
m_constraintRefs.Add(c);
}
m_checkCollideWith = true;
}
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 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 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 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 virtual void RemoveConstraint(TypedConstraint constraint)
{
//(void)constraint;
}
public virtual void AddConstraint(TypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies)
{
//(void)constraint;
//(void)disableCollisionsBetweenLinkedBodies;
}
public virtual void AddConstraint(TypedConstraint constraint)
{
AddConstraint(constraint, false);
}
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;
}
}
}