/// <summary>
		/// Draws the specified constraint.
		/// </summary>
		/// <param name="constraint">The constraint.</param>
		/// <param name="debugDraw">The debug draw.</param>
		public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
		{
			var p6DOF = (Generic6DofConstraint)constraint;
			Matrix tr = p6DOF.GetCalculatedTransformA();
			if (DrawFrames)
			{
				debugDraw.DrawTransform(ref tr, DrawSize);
			}
			tr = p6DOF.GetCalculatedTransformB();
			if (DrawFrames)
			{
				debugDraw.DrawTransform(ref tr, DrawSize);
			}
			Vector3 zero = Vector3.Zero;
			if (DrawLimits)
			{
				tr = p6DOF.GetCalculatedTransformA();
				Vector3 center = p6DOF.GetCalculatedTransformB().Translation;
				// up is axis 1 not 2 ?
				Vector3 up = MathUtil.MatrixColumn(ref tr, 1);
				Vector3 axis = MathUtil.MatrixColumn(ref tr, 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, DrawSize * .9f, minTh, maxTh, minPs, maxPs, ref zero);
				axis = MathUtil.MatrixColumn(ref tr, 1);
				float ay = p6DOF.GetAngle(1);
				float az = p6DOF.GetAngle(2);
				var cy = (float)System.Math.Cos(ay);
				var sy = (float)System.Math.Sin(ay);
				var cz = (float)System.Math.Cos(az);
				var sz = (float)System.Math.Sin(az);
				var ref1 = new Vector3();
				ref1.X = cy * cz * axis.X + cy * sz * axis.Y - sy * axis.Z;
				ref1.Y = -sz * axis.X + cz * axis.Y;
				ref1.Z = cz * sy * axis.X + sz * sy * axis.Y + cy * axis.Z;
				tr = p6DOF.GetCalculatedTransformB();
				Vector3 normal = -MathUtil.MatrixColumn(ref tr, 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, DrawSize, DrawSize, -MathUtil.SIMD_PI, MathUtil.SIMD_PI,
					                  ref zero, false);
				}
				else if (minFi < maxFi)
				{
					debugDraw.DrawArc(ref center, ref normal, ref ref1, DrawSize, DrawSize, minFi, maxFi, ref zero, false);
				}
				tr = p6DOF.GetCalculatedTransformA();
				Vector3 bbMin = p6DOF.GetTranslationalLimitMotor().m_lowerLimit;
				Vector3 bbMax = p6DOF.GetTranslationalLimitMotor().m_upperLimit;
				debugDraw.DrawBox(ref bbMin, ref bbMax, ref tr, ref zero);
			}
		}
		public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
		{
			var pCT = (ConeTwistConstraint)constraint;
			Matrix tr = MathUtil.BulletMatrixMultiply(pCT.GetRigidBodyA().GetCenterOfMassTransform(), pCT.GetAFrame());
			if (DrawFrames) debugDraw.DrawTransform(ref tr, DrawSize);
			tr = MathUtil.BulletMatrixMultiply(pCT.GetRigidBodyB().GetCenterOfMassTransform(), pCT.GetBFrame());
			if (DrawFrames) debugDraw.DrawTransform(ref tr, DrawSize);
			Vector3 zero = Vector3.Zero;

			if (DrawLimits)
			{
				//const float length = float(5);
				float length = DrawSize;
				int nSegments = 8 * 4;
				float fAngleInRadians = MathUtil.SIMD_2_PI * (nSegments - 1) / nSegments;
				Vector3 pPrev = pCT.GetPointForAngle(fAngleInRadians, length);
				pPrev = Vector3.Transform(pPrev, tr);
				for (int i = 0; i < nSegments; i++)
				{
					fAngleInRadians = MathUtil.SIMD_2_PI * i / nSegments;
					Vector3 pCur = pCT.GetPointForAngle(fAngleInRadians, length);
					pCur = Vector3.Transform(pCur, tr);
					debugDraw.DrawLine(ref pPrev, ref pCur, ref zero);

					if (i % (nSegments / 8) == 0)
					{
						Vector3 origin = tr.Translation;
						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 = MathUtil.BulletMatrixMultiply(pCT.GetRigidBodyB().GetCenterOfMassTransform(), pCT.GetBFrame());
				}
				else
				{
					tr = MathUtil.BulletMatrixMultiply(pCT.GetRigidBodyA().GetCenterOfMassTransform(), pCT.GetAFrame());
				}
				Vector3 pivot = tr.Translation;
				Vector3 normal = MathUtil.MatrixColumn(ref tr, 0);
				Vector3 axis1 = MathUtil.MatrixColumn(ref tr, 1);

				debugDraw.DrawArc(ref pivot, ref normal, ref axis1, DrawSize, DrawSize, -twa - tws, -twa + tws, ref zero, true);
			}
		}
		public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
		{
			var p2pC = (Point2PointConstraint)constraint;
			Matrix tr = Matrix.Identity;
			Vector3 pivot = p2pC.GetPivotInA();
			pivot = Vector3.Transform(pivot, p2pC.GetRigidBodyA().GetCenterOfMassTransform());
			tr.Translation = pivot;
			debugDraw.DrawTransform(ref tr, DrawSize);
			// that ideally should draw the same frame	
			pivot = p2pC.GetPivotInB();
			pivot = Vector3.Transform(pivot, p2pC.GetRigidBodyB().GetCenterOfMassTransform());
			tr.Translation = pivot;

			if (DrawFrames)
				debugDraw.DrawTransform(ref tr, DrawSize);
		}
		public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
		{
			var pSlider = (SliderConstraint)constraint;
			Matrix tr = pSlider.GetCalculatedTransformA();
			if (DrawFrames) debugDraw.DrawTransform(ref tr, DrawSize);
			tr = pSlider.GetCalculatedTransformB();
			if (DrawFrames) debugDraw.DrawTransform(ref tr, DrawSize);
			Vector3 zero = Vector3.Zero;
			if (DrawLimits)
			{
				Matrix tr2 = pSlider.GetCalculatedTransformA();
				Vector3 li_min = Vector3.Transform(new Vector3(pSlider.GetLowerLinLimit(), 0f, 0f), tr2);
				Vector3 li_max = Vector3.Transform(new Vector3(pSlider.GetUpperLinLimit(), 0f, 0f), tr2);
				debugDraw.DrawLine(ref li_min, ref li_max, ref zero);
				Vector3 normal = MathUtil.MatrixColumn(ref tr, 0);
				Vector3 axis = MathUtil.MatrixColumn(ref tr, 1);
				float a_min = pSlider.GetLowerAngLimit();
				float a_max = pSlider.GetUpperAngLimit();
				Vector3 center = pSlider.GetCalculatedTransformB().Translation;
				debugDraw.DrawArc(ref center, ref normal, ref axis, DrawSize, DrawSize, a_min, a_max, ref zero, true);
			}
		}
		public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
		{
			var pHinge = (HingeConstraint)constraint;
			Matrix tr = MathUtil.BulletMatrixMultiply(pHinge.GetRigidBodyA().GetCenterOfMassTransform(), pHinge.GetAFrame());

			if (DrawFrames)
				debugDraw.DrawTransform(ref tr, DrawSize);

			tr = MathUtil.BulletMatrixMultiply(pHinge.GetRigidBodyB().GetCenterOfMassTransform(), pHinge.GetBFrame());

			if (DrawFrames)
				debugDraw.DrawTransform(ref tr, DrawSize);

			float minAng = pHinge.GetLowerLimit();
			float maxAng = pHinge.GetUpperLimit();

			if (minAng == maxAng)
				return;

			bool drawSect = true;

			if (minAng > maxAng)
			{
				minAng = 0f;
				maxAng = MathUtil.SIMD_2_PI;
				drawSect = false;
			}

			if (DrawLimits)
			{
				Vector3 center = tr.Translation;
				Vector3 normal = MathUtil.MatrixColumn(ref tr, 2);
				Vector3 axis = MathUtil.MatrixColumn(ref tr, 0);
				Vector3 zero = Vector3.Zero;
				debugDraw.DrawArc(ref center, ref normal, ref axis, DrawSize, DrawSize, minAng, maxAng, ref zero, drawSect);
			}
		}
Exemple #6
0
		public virtual void	RemoveConstraint(TypedConstraint constraint)
        {
            //(void)constraint;
        }
Exemple #7
0
		public virtual void	AddConstraint(TypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies) 
		{ 
            //(void)constraint; 
            //(void)disableCollisionsBetweenLinkedBodies;
		}
Exemple #8
0
 public virtual void AddConstraint(TypedConstraint constraint)
 {
     AddConstraint(constraint, false);
 }
		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 override void RemoveConstraint(TypedConstraint constraint)
		{
			m_constraints.Remove(constraint);
			constraint.GetRigidBodyA().RemoveConstraintRef(constraint);
			constraint.GetRigidBodyB().RemoveConstraintRef(constraint);
		}
		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);
			}
		}
Exemple #12
0
		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);
		}
Exemple #13
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_solverConstraints.Length,info2.fps,info2.erp,info2.findex,info2.m_numIterations));
				for(int i=0;i<info2.m_solverConstraints.Length;++i)
				{
					writer.WriteLine(String.Format("SolverConstaint[{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);

				}
			}

		}
Exemple #14
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));
			}

		}
Exemple #15
0
		public void RemoveConstraintRef(TypedConstraint c)
		{
			m_constraintRefs.Remove(c);
			m_checkCollideWith = m_constraintRefs.Count > 0;

		}
Exemple #16
0
		public void AddConstraintRef(TypedConstraint c)
		{
			if (!m_constraintRefs.Contains(c))
			{
				m_constraintRefs.Add(c);
			}

			m_checkCollideWith = true;
		}