Пример #1
0
        public HingeConstraint(RigidBody rbA, ref IndexedMatrix rbAFrame, bool useReferenceFrameA)
            : base(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA)
        {
            m_rbAFrame                    = rbAFrame;
            m_rbBFrame                    = rbAFrame;
            m_angularOnly                 = false;
            m_enableAngularMotor          = false;
            m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET;

            m_useReferenceFrameA = useReferenceFrameA;
            m_flags = 0;

            m_rbBFrame._origin = m_rbA.GetCenterOfMassTransform() * (m_rbAFrame._origin);

#if     _BT_USE_CENTER_LIMIT_
            m_limit = new AngularLimit();
#else
            //start with free
            m_lowerLimit       = 1f;
            m_upperLimit       = -1f;
            m_biasFactor       = 0.3f;
            m_relaxationFactor = 1.0f;
            m_limitSoftness    = 0.9f;
            m_solveLimit       = false;
#endif
            m_referenceSign = m_useReferenceFrameA ? -1.0f : 1.0f;
        }
Пример #2
0
        public HingeConstraint(RigidBody rbA, ref IndexedVector3 pivotInA, ref IndexedVector3 axisInA, bool useReferenceFrameA)
            : base(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA)
        {
            m_angularOnly                 = false;
            m_enableAngularMotor          = false;
            m_useReferenceFrameA          = useReferenceFrameA;
            m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET;
            m_flags = 0;

#if     _BT_USE_CENTER_LIMIT_
            m_limit = new AngularLimit();
#endif

            // since no frame is given, assume this to be zero angle and just pick rb transform axis
            // fixed axis in worldspace
            IndexedVector3 rbAxisA1, rbAxisA2;
            TransformUtil.PlaneSpace1(ref axisInA, out rbAxisA1, out rbAxisA2);

            m_rbAFrame._origin = pivotInA;
            //m_rbAFrame._basis = new IndexedBasisMatrix(ref rbAxisA1, ref rbAxisA2, ref axisInA);
            m_rbAFrame._basis = new IndexedBasisMatrix(rbAxisA1.X, rbAxisA2.X, axisInA.X,
                                                       rbAxisA1.Y, rbAxisA2.Y, axisInA.Y,
                                                       rbAxisA1.Z, rbAxisA2.Z, axisInA.Z);

            IndexedVector3 axisInB = rbA.GetCenterOfMassTransform()._basis *axisInA;

            IndexedQuaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB);
            IndexedVector3    rbAxisB1    = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1);
            IndexedVector3    rbAxisB2    = IndexedVector3.Cross(axisInB, rbAxisB1);

            m_rbBFrame._origin = rbA.GetCenterOfMassTransform() * pivotInA;
            //m_rbBFrame._basis = new IndexedBasisMatrix(ref rbAxisB1, ref rbAxisB2, ref axisInB);
            m_rbBFrame._basis = new IndexedBasisMatrix(rbAxisB1.X, rbAxisB2.X, axisInB.X,
                                                       rbAxisB1.Y, rbAxisB2.Y, axisInB.Y,
                                                       rbAxisB1.Z, rbAxisB2.Z, axisInB.Z);

            //start with free
#if !_BT_USE_CENTER_LIMIT_
            m_lowerLimit       = 1f;
            m_upperLimit       = -1f;
            m_biasFactor       = 0.3f;
            m_relaxationFactor = 1.0f;
            m_limitSoftness    = 0.9f;
            m_solveLimit       = false;
#endif
            m_referenceSign = m_useReferenceFrameA ? -1.0f : 1.0f;
        }
		public HingeConstraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 pivotInA, ref IndexedVector3 pivotInB, ref IndexedVector3 axisInA, ref IndexedVector3 axisInB, bool useReferenceFrameA)
			: base(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA, rbB)
		{
			m_angularOnly = false;
			m_enableAngularMotor = false;
			m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET;

			m_useReferenceFrameA = useReferenceFrameA;

			m_rbAFrame._origin = pivotInA;
#if	_BT_USE_CENTER_LIMIT_
			m_limit = new AngularLimit();
#endif


			m_flags = 0;

			// since no frame is given, assume this to be zero angle and just pick rb transform axis
            IndexedVector3 rbAxisA1 = rbA.GetCenterOfMassTransform()._basis.GetColumn(0);

			IndexedVector3 rbAxisA2 = IndexedVector3.Zero;
			float projection = IndexedVector3.Dot(axisInA, rbAxisA1);
			if (projection >= 1.0f - MathUtil.SIMD_EPSILON)
			{
                rbAxisA1 = -rbA.GetCenterOfMassTransform()._basis.GetColumn(2);
                rbAxisA2 = rbA.GetCenterOfMassTransform()._basis.GetColumn(1);
            }
			else if (projection <= -1.0f + MathUtil.SIMD_EPSILON)
			{
                rbAxisA1 = rbA.GetCenterOfMassTransform()._basis.GetColumn(2);
                rbAxisA2 = rbA.GetCenterOfMassTransform()._basis.GetColumn(1);
            }
			else
			{
				rbAxisA2 = IndexedVector3.Cross(axisInA, rbAxisA1);
				rbAxisA1 = IndexedVector3.Cross(rbAxisA2, axisInA);
			}

            
            //m_rbAFrame._basis = new IndexedBasisMatrix(ref rbAxisA1, ref rbAxisA2, ref axisInA);
            m_rbAFrame._basis = new IndexedBasisMatrix(rbAxisA1.X, rbAxisA2.X, axisInA.X,
                                    rbAxisA1.Y, rbAxisA2.Y, axisInA.Y,
                                    rbAxisA1.Z, rbAxisA2.Z, axisInA.Z);

			IndexedQuaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB);
			IndexedVector3 rbAxisB1 = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1);
			IndexedVector3 rbAxisB2 = IndexedVector3.Cross(axisInB, rbAxisB1);

			m_rbBFrame._origin = pivotInB;
            //m_rbBFrame._basis = new IndexedBasisMatrix(ref rbAxisB1, ref rbAxisB2, ref axisInB);
            m_rbBFrame._basis = new IndexedBasisMatrix(rbAxisB1.X, rbAxisB2.X, axisInB.X,
                                    rbAxisB1.Y, rbAxisB2.Y, axisInB.Y,
                                    rbAxisB1.Z, rbAxisB2.Z, axisInB.Z);

#if!	_BT_USE_CENTER_LIMIT_
	//start with free
	m_lowerLimit = float(1.0f);
	m_upperLimit = float(-1.0f);
	m_biasFactor = 0.3f;
	m_relaxationFactor = 1.0f;
	m_limitSoftness = 0.9f;
	m_solveLimit = false;
#endif
			m_referenceSign = m_useReferenceFrameA ? -1f : 1f;
		}
		public HingeConstraint(RigidBody rbA, ref IndexedMatrix rbAFrame, bool useReferenceFrameA)
			: base(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA)
		{
			m_rbAFrame = rbAFrame;
			m_rbBFrame = rbAFrame;
			m_angularOnly = false;
			m_enableAngularMotor = false;
			m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET;

			m_useReferenceFrameA = useReferenceFrameA;
			m_flags = 0;

            m_rbBFrame._origin = m_rbA.GetCenterOfMassTransform() * (m_rbAFrame._origin);

#if	_BT_USE_CENTER_LIMIT_
			m_limit = new AngularLimit();
#else

            //start with free
            m_lowerLimit = 1f;
            m_upperLimit = -1f;
            m_biasFactor = 0.3f;
            m_relaxationFactor = 1.0f;
            m_limitSoftness = 0.9f;
            m_solveLimit = false;
#endif
			m_referenceSign = m_useReferenceFrameA ? -1.0f : 1.0f;

		}
		public HingeConstraint(RigidBody rbA, ref IndexedVector3 pivotInA, ref IndexedVector3 axisInA, bool useReferenceFrameA)
			: base(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA)
		{
			m_angularOnly = false;
			m_enableAngularMotor = false;
			m_useReferenceFrameA = useReferenceFrameA;
			m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET;
			m_flags = 0;

#if	_BT_USE_CENTER_LIMIT_
			m_limit = new AngularLimit();
#endif

			// since no frame is given, assume this to be zero angle and just pick rb transform axis
			// fixed axis in worldspace
			IndexedVector3 rbAxisA1, rbAxisA2;
			TransformUtil.PlaneSpace1(ref axisInA, out rbAxisA1, out rbAxisA2);

			m_rbAFrame._origin = pivotInA;
            //m_rbAFrame._basis = new IndexedBasisMatrix(ref rbAxisA1, ref rbAxisA2, ref axisInA);
            m_rbAFrame._basis = new IndexedBasisMatrix(rbAxisA1.X, rbAxisA2.X, axisInA.X,
                                                rbAxisA1.Y, rbAxisA2.Y, axisInA.Y,
                                                rbAxisA1.Z, rbAxisA2.Z, axisInA.Z);

            IndexedVector3 axisInB = rbA.GetCenterOfMassTransform()._basis * axisInA;

			IndexedQuaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB);
			IndexedVector3 rbAxisB1 = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1);
			IndexedVector3 rbAxisB2 = IndexedVector3.Cross(axisInB, rbAxisB1);

            m_rbBFrame._origin = rbA.GetCenterOfMassTransform() * pivotInA;
            //m_rbBFrame._basis = new IndexedBasisMatrix(ref rbAxisB1, ref rbAxisB2, ref axisInB);
            m_rbBFrame._basis = new IndexedBasisMatrix(rbAxisB1.X, rbAxisB2.X, axisInB.X,
                        rbAxisB1.Y, rbAxisB2.Y, axisInB.Y,
                        rbAxisB1.Z, rbAxisB2.Z, axisInB.Z);

			//start with free
#if!	_BT_USE_CENTER_LIMIT_

            m_lowerLimit = 1f;
            m_upperLimit = -1f;
            m_biasFactor = 0.3f;
            m_relaxationFactor = 1.0f;
            m_limitSoftness = 0.9f;
            m_solveLimit = false;
#endif
			m_referenceSign = m_useReferenceFrameA ? -1.0f : 1.0f;
		}
Пример #6
0
        public HingeConstraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 pivotInA, ref IndexedVector3 pivotInB, ref IndexedVector3 axisInA, ref IndexedVector3 axisInB, bool useReferenceFrameA)
            : base(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA, rbB)
        {
            m_angularOnly                 = false;
            m_enableAngularMotor          = false;
            m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET;

            m_useReferenceFrameA = useReferenceFrameA;

            m_rbAFrame._origin = pivotInA;
#if     _BT_USE_CENTER_LIMIT_
            m_limit = new AngularLimit();
#endif


            m_flags = 0;

            // since no frame is given, assume this to be zero angle and just pick rb transform axis
            IndexedVector3 rbAxisA1 = rbA.GetCenterOfMassTransform()._basis.GetColumn(0);

            IndexedVector3 rbAxisA2   = IndexedVector3.Zero;
            float          projection = IndexedVector3.Dot(axisInA, rbAxisA1);
            if (projection >= 1.0f - MathUtil.SIMD_EPSILON)
            {
                rbAxisA1 = -rbA.GetCenterOfMassTransform()._basis.GetColumn(2);
                rbAxisA2 = rbA.GetCenterOfMassTransform()._basis.GetColumn(1);
            }
            else if (projection <= -1.0f + MathUtil.SIMD_EPSILON)
            {
                rbAxisA1 = rbA.GetCenterOfMassTransform()._basis.GetColumn(2);
                rbAxisA2 = rbA.GetCenterOfMassTransform()._basis.GetColumn(1);
            }
            else
            {
                rbAxisA2 = IndexedVector3.Cross(axisInA, rbAxisA1);
                rbAxisA1 = IndexedVector3.Cross(rbAxisA2, axisInA);
            }


            //m_rbAFrame._basis = new IndexedBasisMatrix(ref rbAxisA1, ref rbAxisA2, ref axisInA);
            m_rbAFrame._basis = new IndexedBasisMatrix(rbAxisA1.X, rbAxisA2.X, axisInA.X,
                                                       rbAxisA1.Y, rbAxisA2.Y, axisInA.Y,
                                                       rbAxisA1.Z, rbAxisA2.Z, axisInA.Z);

            IndexedQuaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB);
            IndexedVector3    rbAxisB1    = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1);
            IndexedVector3    rbAxisB2    = IndexedVector3.Cross(axisInB, rbAxisB1);

            m_rbBFrame._origin = pivotInB;
            //m_rbBFrame._basis = new IndexedBasisMatrix(ref rbAxisB1, ref rbAxisB2, ref axisInB);
            m_rbBFrame._basis = new IndexedBasisMatrix(rbAxisB1.X, rbAxisB2.X, axisInB.X,
                                                       rbAxisB1.Y, rbAxisB2.Y, axisInB.Y,
                                                       rbAxisB1.Z, rbAxisB2.Z, axisInB.Z);

#if !_BT_USE_CENTER_LIMIT_
            //start with free
            m_lowerLimit       = float(1.0f);
            m_upperLimit       = float(-1.0f);
            m_biasFactor       = 0.3f;
            m_relaxationFactor = 1.0f;
            m_limitSoftness    = 0.9f;
            m_solveLimit       = false;
#endif
            m_referenceSign = m_useReferenceFrameA ? -1f : 1f;
        }