Esempio n. 1
0
 //!@}
 public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, btTransform frameInA, btTransform frameInB, bool useLinearReferenceFrameA)
     : base(TypedConstraintType.D6_CONSTRAINT_TYPE, rbA, rbB)
 {
     m_linearLimits = new TranslationalLimitMotor();
     m_angularLimits = new RotationalLimitMotor[3];
     for (int i = 0; i < m_angularLimits.Length; i++)
         m_angularLimits[i] = new RotationalLimitMotor();
     m_frameInA = frameInA;
     m_frameInB = frameInB;
     m_useLinearReferenceFrameA = useLinearReferenceFrameA;
     m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET;
     m_flags = bt6DofFlags.BT_6DOF_FLAGS_NONE;
     m_useSolveConstraintObsolete = D6_USE_OBSOLETE_METHOD;
     calculateTransforms();
 }
Esempio n. 2
0
 public Generic6DofConstraint(RigidBody rbB, btTransform frameInB, bool useLinearReferenceFrameB)
     : base(TypedConstraintType.D6_CONSTRAINT_TYPE, getFixedBody(), rbB)
 {
     m_linearLimits = new TranslationalLimitMotor();
     m_angularLimits = new RotationalLimitMotor[3];
     for (int i = 0; i < m_angularLimits.Length; i++)
         m_angularLimits[i] = new RotationalLimitMotor();
     m_frameInB = frameInB;
     m_useLinearReferenceFrameA = useLinearReferenceFrameB;
     m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET;
     m_flags = bt6DofFlags.BT_6DOF_FLAGS_NONE;
     m_useSolveConstraintObsolete = false;
     ///not providing rigidbody A means implicitly using worldspace for body A
     m_frameInA = rbB.CenterOfMassTransform * m_frameInB;
     calculateTransforms();
 }
Esempio n. 3
0
        public int get_limit_motor_info2(
	        RotationalLimitMotor  limot,
	        btTransform transA,btTransform transB,btVector3 linVelA,btVector3 linVelB,btVector3 angVelA,btVector3 angVelB,
	        ConstraintInfo2 info, int row,ref btVector3 ax1, bool rotational,bool rotAllowed)
        {
            //ポインタを使わず、Listを直接受け取っているので、srow=rowとして、Listに入れるように修正
            //int srow = row * info.rowskip;
            int srow = row;
            
                    bool powered = limot.m_enableMotor;
            int limit = limot.m_currentLimit;
            if (powered || limit!=0)
            {   // if the joint is powered, or has joint limits, add in the extra row

                //rotationalでポインタ分けをしていたのを修正。
#if false
                float *J1 = rotational ? info.m_J1angularAxis : info.m_J1linearAxis;
                J1[srow+0] = ax1[0];
                J1[srow+1] = ax1[1];
                J1[srow+2] = ax1[2];
#endif
                if (rotational)
                    info.Constraints[srow + info.CurrentRow].m_relpos1CrossNormal = ax1;
                else
                    info.Constraints[srow + info.CurrentRow].m_contactNormal = ax1;

                //btScalar* J2 = rotational ? info->m_J2angularAxis : 0;
                if (rotational)
                {
#if false
                    J2[srow+0] = -ax1[0];
                    J2[srow+1] = -ax1[1];
                    J2[srow+2] = -ax1[2];
#endif
                    
                    info.Constraints[srow + info.CurrentRow].m_relpos2CrossNormal = -ax1;
                }
                if((!rotational))
                {
			        if (m_useOffsetForConstraintFrame)
                    {
                        btVector3 tmpA, tmpB, relA, relB;
                        // get vector from bodyB to frameB in WCS
                        relB = m_calculatedTransformB.Origin - transB.Origin;
                        // get its projection to constraint axis
                        btVector3 projB = ax1 * relB.dot(ax1);
                        // get vector directed from bodyB to constraint axis (and orthogonal to it)
                        btVector3 orthoB = relB - projB;
                        // same for bodyA
                        relA = m_calculatedTransformA.Origin - transA.Origin;
                        btVector3 projA = ax1 * relA.dot(ax1);
                        btVector3 orthoA = relA - projA;
                        // get desired offset between frames A and B along constraint axis
                        float desiredOffs = limot.m_currentPosition - limot.m_currentLimitError;
                        // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
                        btVector3 totalDist;// = projA + ax1 * desiredOffs - projB;
                        {
                            btVector3 temp1, temp2;
                            btVector3.Multiply(ref ax1, desiredOffs, out temp1);
                            btVector3.Add(ref projA, ref temp1, out temp2);
                            btVector3.Subtract(ref temp2, ref projB, out totalDist);
                        }
                        // get offset vectors relA and relB
                        #region relA = orthoA + totalDist * m_factA;
                        {
                            btVector3 temp;
                            btVector3.Multiply(ref totalDist, m_factA, out temp);
                            btVector3.Add(ref orthoA, ref temp, out relA);
                        }
                        #endregion
                        #region relB = orthoB - totalDist * m_factB;
                        {
                            btVector3 temp;
                            btVector3.Multiply(ref totalDist, m_factB, out temp);
                            btVector3.Subtract(ref orthoB, ref temp, out relB);
                        }
                        #endregion
                        //tmpA = relA.cross(ax1);
                        relA.cross(ref ax1, out tmpA);
                        //tmpB = relB.cross(ax1);
                        relB.cross(ref ax1, out tmpB);
                        if (m_hasStaticBody && (!rotAllowed))
                        {
                            tmpA *= m_factA;
                            tmpB *= m_factB;
                        }
                        //int i;
                        //for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
                        //for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
                        info.Constraints[srow + info.CurrentRow].m_relpos1CrossNormal = tmpA;
                        info.Constraints[srow + info.CurrentRow].m_relpos2CrossNormal = -tmpB;
                    }
                    else
			        {
				        btVector3 ltd;	// Linear Torque Decoupling vector
				        btVector3 c = m_calculatedTransformB.Origin - transA.Origin;
				        ltd = c.cross(ax1);
#if false
				        info->m_J1angularAxis[srow+0] = ltd[0];
				        info->m_J1angularAxis[srow+1] = ltd[1];
				        info->m_J1angularAxis[srow+2] = ltd[2];
#endif
                        info.Constraints[srow + info.CurrentRow].m_relpos1CrossNormal = ltd;


				        c = m_calculatedTransformB.Origin - transB.Origin;
				        ltd = -c.cross(ax1);

#if false
				        info->m_J2angularAxis[srow+0] = ltd[0];
				        info->m_J2angularAxis[srow+1] = ltd[1];
				        info->m_J2angularAxis[srow+2] = ltd[2];
#endif
                        info.Constraints[srow + info.CurrentRow].m_relpos2CrossNormal = ltd;
			        }
                }
                // if we're limited low and high simultaneously, the joint motor is
                // ineffective
                if (limit!=0 && (limot.m_loLimit == limot.m_hiLimit)) powered = false;
                //info->m_constraintError[srow] = 0f;
                info.Constraints[srow + info.CurrentRow].m_rhs = 0f;
                if (powered)
                {
			        //info->cfm[srow] = limot.m_normalCFM;
                    info.Constraints[srow + info.CurrentRow].m_cfm = limot.m_normalCFM;
                    if(limit==0)
                    {
				        float tag_vel = rotational ? limot.m_targetVelocity : -limot.m_targetVelocity;

				        float mot_fact = getMotorFactor(	limot.m_currentPosition, 
													        limot.m_loLimit,
													        limot.m_hiLimit, 
													        tag_vel, 
													        info.fps * limot.m_stopERP);
#if false
				        info->m_constraintError[srow] += mot_fact * limot.m_targetVelocity;
                        info->m_lowerLimit[srow] = -limot.m_maxMotorForce;
                        info->m_upperLimit[srow] = limot.m_maxMotorForce;
#endif
                        info.Constraints[srow + info.CurrentRow].m_rhs+=mot_fact * limot.m_targetVelocity;
                        info.Constraints[srow + info.CurrentRow].m_lowerLimit = -limot.m_maxMotorForce;
                        info.Constraints[srow + info.CurrentRow].m_upperLimit = limot.m_maxMotorForce;
                    }
                }
                if(limit!=0)
                {
                    float k = info.fps * limot.m_stopERP;
			        if(!rotational)
			        {
				        //info->m_constraintError[srow] += k * limot.m_currentLimitError;
                        info.Constraints[srow + info.CurrentRow].m_rhs += k * limot.m_currentLimitError;

			        }
			        else
			        {
				        //info->m_constraintError[srow] += -k * limot.m_currentLimitError;
                        info.Constraints[srow + info.CurrentRow].m_rhs += -k * limot.m_currentLimitError;
			        }
                    //info->cfm[srow] = limot.m_stopCFM;
                    info.Constraints[srow + info.CurrentRow].m_cfm = limot.m_stopCFM;
                    if (limot.m_loLimit == limot.m_hiLimit)
                    {   // limited low and high simultaneously
                        //info->m_lowerLimit[srow] = float.NegativeInfinity;
                        //info->m_upperLimit[srow] = float.PositiveInfinity;
                        info.Constraints[srow + info.CurrentRow].m_lowerLimit = float.NegativeInfinity;
                        info.Constraints[srow + info.CurrentRow].m_upperLimit = float.PositiveInfinity;
                    }
                    else
                    {
                        if (limit == 1)
                        {
                            //info->m_lowerLimit[srow] = 0;
                            //info->m_upperLimit[srow] = float.PositiveInfinity;
                            info.Constraints[srow + info.CurrentRow].m_lowerLimit = 0;
                            info.Constraints[srow + info.CurrentRow].m_upperLimit = float.PositiveInfinity;
                        }
                        else
                        {
                            //info->m_lowerLimit[srow] = float.NegativeInfinity;
                            //info->m_upperLimit[srow] = 0;
                            info.Constraints[srow + info.CurrentRow].m_lowerLimit = float.NegativeInfinity;
                            info.Constraints[srow + info.CurrentRow].m_upperLimit = 0;
                        }
                        // deal with bounce
                        if (limot.m_bounce > 0)
                        {
                            // calculate joint velocity
                            float vel;
                            if (rotational)
                            {
                                vel = angVelA.dot(ax1);
        //make sure that if no body -> angVelB == zero vec
        //                        if (body1)
                                    vel -= angVelB.dot(ax1);
                            }
                            else
                            {
                                vel = linVelA.dot(ax1);
        //make sure that if no body -> angVelB == zero vec
        //                        if (body1)
                                    vel -= linVelB.dot(ax1);
                            }
                            // only apply bounce if the velocity is incoming, and if the
                            // resulting c[] exceeds what we already have.
                            if (limit == 1)
                            {
                                if (vel < 0)
                                {
                                    float newc = -limot.m_bounce* vel;
                                    //if (newc > info->m_constraintError[srow])
                                    //    info->m_constraintError[srow] = newc;
                                    if (newc > info.Constraints[srow + info.CurrentRow].m_rhs)
                                        info.Constraints[srow + info.CurrentRow].m_rhs = newc;
                                }
                            }
                            else
                            {
                                if (vel > 0)
                                {
                                    float newc = -limot.m_bounce * vel;
                                    //if (newc < info->m_constraintError[srow])
                                    //    info->m_constraintError[srow] = newc;
                                    if (newc < info.Constraints[srow + info.CurrentRow].m_rhs)
                                        info.Constraints[srow + info.CurrentRow].m_rhs = newc;
                                }
                            }
                        }
                    }
                }
                return 1;
            }
            else return 0;
        }
Esempio n. 4
0
     public int get_limit_motor_info2(
 RotationalLimitMotor limot,
 btTransform transA, btTransform transB, btVector3 linVelA, btVector3 linVelB, btVector3 angVelA, btVector3 angVelB,
 ConstraintInfo2 info, int row, ref btVector3 ax1, bool rotational)
     {
         return get_limit_motor_info2(limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, ref ax1, rotational, false);
     }