public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, ref Matrix frameInA, ref Matrix frameInB ,bool useLinearReferenceFrameA) : base(TypedConstraintType.D6_CONSTRAINT_TYPE,rbA,rbB) { m_frameInA = frameInA; m_frameInB = frameInB; m_useLinearReferenceFrameA = useLinearReferenceFrameA; m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET; m_flags = 0; m_linearLimits = new TranslationalLimitMotor(); m_angularLimits[0] = new RotationalLimitMotor(); m_angularLimits[1] = new RotationalLimitMotor(); m_angularLimits[2] = new RotationalLimitMotor(); CalculateTransforms(); }
public RotationalLimitMotor(RotationalLimitMotor limot) { m_targetVelocity = limot.m_targetVelocity; m_maxMotorForce = limot.m_maxMotorForce; m_limitSoftness = limot.m_limitSoftness; m_loLimit = limot.m_loLimit; m_hiLimit = limot.m_hiLimit; m_normalCFM = limot.m_normalCFM; m_stopERP = limot.m_stopERP; m_stopCFM = limot.m_stopCFM; m_bounce = limot.m_bounce; m_currentLimit = limot.m_currentLimit; m_currentLimitError = limot.m_currentLimitError; m_enableMotor = limot.m_enableMotor; }
protected Generic6DofConstraint(RigidBody rbB, ref Matrix frameInB, bool useLinearReferenceFrameB) : base(TypedConstraintType.D6_CONSTRAINT_TYPE,GetFixedBody(),rbB) { m_frameInB = frameInB; m_useLinearReferenceFrameA = useLinearReferenceFrameB; m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET; m_flags = 0; m_linearLimits = new TranslationalLimitMotor(); m_angularLimits[0] = new RotationalLimitMotor(); m_angularLimits[1] = new RotationalLimitMotor(); m_angularLimits[2] = new RotationalLimitMotor(); ///not providing rigidbody A means implicitly using worldspace for body A m_frameInA = MathUtil.BulletMatrixMultiply(rbB.GetCenterOfMassTransform(),m_frameInB); CalculateTransforms(); }
protected virtual int SetLinearLimits(ConstraintInfo2 info, int row, ref Matrix transA,ref Matrix transB,ref Vector3 linVelA,ref Vector3 linVelB,ref Vector3 angVelA,ref Vector3 angVelB) { //solve linear limits RotationalLimitMotor limot = new RotationalLimitMotor(); for (int i = 0; i < 3; i++) { if (m_linearLimits.NeedApplyForce(i)) { // re-use rotational motor code limot.m_bounce = 0f; limot.m_currentLimit = m_linearLimits.m_currentLimit[i]; limot.m_currentPosition = MathUtil.VectorComponent(ref m_linearLimits.m_currentLinearDiff,i); limot.m_currentLimitError = MathUtil.VectorComponent(ref m_linearLimits.m_currentLimitError,i); limot.m_damping = m_linearLimits.m_damping; limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; limot.m_hiLimit = MathUtil.VectorComponent(m_linearLimits.m_upperLimit,i); limot.m_limitSoftness = m_linearLimits.m_limitSoftness; limot.m_loLimit = MathUtil.VectorComponent(m_linearLimits.m_lowerLimit,i); limot.m_maxLimitForce = 0f; limot.m_maxMotorForce = MathUtil.VectorComponent(m_linearLimits.m_maxMotorForce,i); limot.m_targetVelocity = MathUtil.VectorComponent(m_linearLimits.m_targetVelocity,i); Vector3 axis = MathUtil.MatrixColumn(m_calculatedTransformA,i); int tempFlags = (((int)m_flags) >> (i * BT_6DOF_FLAGS_AXIS_SHIFT)); SixDofFlags flags = (SixDofFlags)tempFlags; limot.m_normalCFM = ((flags & SixDofFlags.BT_6DOF_FLAGS_CFM_NORM) != 0) ? MathUtil.VectorComponent(ref m_linearLimits.m_normalCFM,i) : info.m_solverConstraints[0].m_cfm; limot.m_stopCFM = ((flags & SixDofFlags.BT_6DOF_FLAGS_CFM_STOP) != 0) ? MathUtil.VectorComponent(ref m_linearLimits.m_stopCFM,i) : info.m_solverConstraints[0].m_cfm; limot.m_stopERP = ((flags & SixDofFlags.BT_6DOF_FLAGS_ERP_STOP) != 0) ? MathUtil.VectorComponent(ref m_linearLimits.m_stopERP,i) : info.erp; if(m_useOffsetForConstraintFrame) { int indx1 = (i + 1) % 3; int indx2 = (i + 2) % 3; bool rotAllowed = true; // rotations around orthos to current axis if(m_angularLimits[indx1].m_currentLimit != 0 && m_angularLimits[indx2].m_currentLimit != 0) { rotAllowed = false; } row += GetLimitMotorInfo2(limot, ref transA,ref transB,ref linVelA,ref linVelB,ref angVelA,ref angVelB, info, row, ref axis, 0, rotAllowed); } else { row += GetLimitMotorInfo2(limot, ref transA,ref transB,ref linVelA,ref linVelB,ref angVelA,ref angVelB, info, row, ref axis, 0,false); } } } return row; }
public virtual int GetLimitMotorInfo2(RotationalLimitMotor limot, ref Matrix transA,ref Matrix transB,ref Vector3 linVelA, ref Vector3 linVelB,ref Vector3 angVelA,ref Vector3 angVelB, ConstraintInfo2 info, int row, ref Vector3 ax1, int rotational,bool rotAllowed) { 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 //btScalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; //btScalar* J2 = rotational ? info->m_J2angularAxis : 0; //info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; //info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; //info2.m_J2linearAxis = 0; //info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; if(rotational != 0) { info.m_solverConstraints[row].m_relpos1CrossNormal = ax1; MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_relpos1CrossNormal); } else { info.m_solverConstraints[row].m_contactNormal = ax1; MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_contactNormal); } if (rotational != 0) { info.m_solverConstraints[row].m_relpos2CrossNormal = -ax1; } //MathUtil.zeroCheckVector(info.m_solverConstraints[row].m_relpos2CrossNormal); if (rotational == 0) { if (m_useOffsetForConstraintFrame) { Vector3 tmpA = Vector3.Zero, tmpB= Vector3.Zero, relA= Vector3.Zero, relB= Vector3.Zero; // get vector from bodyB to frameB in WCS relB = m_calculatedTransformB.Translation - transB.Translation; // get its projection to constraint axis Vector3 projB = ax1 * Vector3.Dot(relB,ax1); // get vector directed from bodyB to constraint axis (and orthogonal to it) Vector3 orthoB = relB - projB; // same for bodyA relA = m_calculatedTransformA.Translation - transA.Translation; Vector3 projA = ax1 * Vector3.Dot(relA,ax1); Vector3 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 Vector3 totalDist = projA + ax1 * desiredOffs - projB; // get offset vectors relA and relB relA = orthoA + totalDist * m_factA; relB = orthoB - totalDist * m_factB; tmpA = Vector3.Cross(relA,ax1); tmpB = Vector3.Cross(relB,ax1); if(m_hasStaticBody && (!rotAllowed)) { tmpA *= m_factA; tmpB *= m_factB; } info.m_solverConstraints[row].m_relpos1CrossNormal = tmpA; MathUtil.ZeroCheckVector(ref tmpA); info.m_solverConstraints[row].m_relpos2CrossNormal = -tmpB; MathUtil.ZeroCheckVector(ref tmpB); } else { Vector3 ltd; // Linear Torque Decoupling vector Vector3 c = m_calculatedTransformB.Translation - transA.Translation; ltd = Vector3.Cross(c,ax1); info.m_solverConstraints[row].m_relpos1CrossNormal = ltd; MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_relpos1CrossNormal); c = m_calculatedTransformB.Translation - transB.Translation; ltd = -Vector3.Cross(c,ax1); info.m_solverConstraints[row].m_relpos2CrossNormal = ltd; MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_relpos2CrossNormal); } } // if we're limited low and high simultaneously, the joint motor is // ineffective if (limit != 0 && (MathUtil.CompareFloat(limot.m_loLimit,limot.m_hiLimit))) { powered = false; } info.m_solverConstraints[row].m_rhs = 0f; if (powered) { info.m_solverConstraints[row].m_cfm = limot.m_normalCFM; if (limit == 0) { float tag_vel = (rotational != 0)? 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); info.m_solverConstraints[row].m_rhs += mot_fact * limot.m_targetVelocity; info.m_solverConstraints[row].m_lowerLimit = -limot.m_maxMotorForce; info.m_solverConstraints[row].m_upperLimit = limot.m_maxMotorForce; } } if (limit != 0) { float k = info.fps * limot.m_stopERP; if (rotational == 0) { info.m_solverConstraints[row].m_rhs += k * limot.m_currentLimitError; } else { info.m_solverConstraints[row].m_rhs += -k * limot.m_currentLimitError; } info.m_solverConstraints[row].m_cfm = limot.m_stopCFM; if (MathUtil.CompareFloat(limot.m_loLimit,limot.m_hiLimit)) { // limited low and high simultaneously info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY; } else { if (limit == 1) { info.m_solverConstraints[row].m_lowerLimit = 0; info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY; } else { info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[row].m_upperLimit = 0; } // deal with bounce if (limot.m_bounce > 0) { // calculate joint velocity float vel; if (rotational != 0) { vel = Vector3.Dot(angVelA,ax1); vel -= Vector3.Dot(angVelB,ax1); } else { vel = Vector3.Dot(linVelA,ax1); vel -= Vector3.Dot(linVelB,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_solverConstraints[row].m_rhs) { info.m_solverConstraints[row].m_rhs = newc; } } } else { if (vel > 0) { float newc = -limot.m_bounce * vel; if (newc < info.m_solverConstraints[row].m_rhs) { info.m_solverConstraints[row].m_rhs = newc; } } } } } } return 1; } else return 0; }