Ejemplo n.º 1
0
        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();
        }
Ejemplo n.º 2
0
        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();
        }
Ejemplo n.º 3
0
        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);
        }
Ejemplo n.º 4
0
        public void AddConstraintRef(TypedConstraint c)
        {
            if (!m_constraintRefs.Contains(c))
            {
                m_constraintRefs.Add(c);
            }

            m_checkCollideWith = true;
        }
Ejemplo n.º 5
0
 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));
     }
 }
Ejemplo n.º 6
0
 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);
     }
 }
Ejemplo n.º 7
0
 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);
         }
     }
 }
Ejemplo n.º 8
0
        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);
        }
Ejemplo n.º 9
0
 public override void RemoveConstraint(TypedConstraint constraint)
 {
     m_constraints.Remove(constraint);
     constraint.GetRigidBodyA().RemoveConstraintRef(constraint);
     constraint.GetRigidBodyB().RemoveConstraintRef(constraint);
 }
Ejemplo n.º 10
0
        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;
		}
Ejemplo n.º 14
0
 public void RemoveConstraintRef(TypedConstraint c)
 {
     m_constraintRefs.Remove(c);
     m_checkCollideWith = m_constraintRefs.Count > 0;
 }
Ejemplo n.º 15
0
        public void RemoveConstraintRef(TypedConstraint c)
        {
            m_constraintRefs.Remove(c);
            m_checkCollideWith = m_constraintRefs.Count > 0;

        }
Ejemplo n.º 16
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);
        }
Ejemplo n.º 18
0
		public virtual void	AddConstraint(TypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies) 
		{ 
            //(void)constraint; 
            //(void)disableCollisionsBetweenLinkedBodies;
		}
Ejemplo n.º 19
0
 public BulletConstraintXNA(TypedConstraint xx) : base()
 {
     constrain = xx;
 }
Ejemplo n.º 20
0
		public virtual void	RemoveConstraint(TypedConstraint constraint)
        {
            //(void)constraint;
        }
Ejemplo n.º 21
0
 public virtual void     RemoveConstraint(TypedConstraint constraint)
 {
     //(void)constraint;
 }
Ejemplo n.º 22
0
 public virtual void     AddConstraint(TypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies)
 {
     //(void)constraint;
     //(void)disableCollisionsBetweenLinkedBodies;
 }
Ejemplo n.º 23
0
 public virtual void AddConstraint(TypedConstraint constraint)
 {
     AddConstraint(constraint, false);
 }
Ejemplo n.º 24
0
 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;
                }
            }
        }