public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, ref IndexedMatrix frameInA, ref IndexedMatrix 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_linearLimits = new TranslationalLimitMotor(); m_angularLimits[0] = new RotationalLimitMotor(); m_angularLimits[1] = new RotationalLimitMotor(); m_angularLimits[2] = new RotationalLimitMotor(); CalculateTransforms(); }
public Generic6DofConstraint(RigidBody rbB, ref IndexedMatrix frameInB, bool useLinearReferenceFrameB) : base(TypedConstraintType.D6_CONSTRAINT_TYPE, GetFixedBody(), rbB) { m_frameInB = frameInB; m_useLinearReferenceFrameA = useLinearReferenceFrameB; m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET; 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 = rbB.GetCenterOfMassTransform() * m_frameInB; CalculateTransforms(); }
protected virtual int SetLinearLimits(ConstraintInfo2 info, int row, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 linVelA, ref IndexedVector3 linVelB, ref IndexedVector3 angVelA, ref IndexedVector3 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 = m_linearLimits.m_currentLinearDiff[i]; limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i]; limot.m_damping = m_linearLimits.m_damping; limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; limot.m_limitSoftness = m_linearLimits.m_limitSoftness; limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; limot.m_maxLimitForce = 0.0f; limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; IndexedVector3 axis = m_calculatedTransformA._basis.GetColumn(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) ? m_linearLimits.m_normalCFM[i] : info.m_solverConstraints[0].m_cfm; limot.m_stopCFM = ((flags & SixDofFlags.BT_6DOF_FLAGS_CFM_STOP) != 0) ? m_linearLimits.m_stopCFM[i] : info.m_solverConstraints[0].m_cfm; limot.m_stopERP = ((flags & SixDofFlags.BT_6DOF_FLAGS_ERP_STOP) != 0) ? 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 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; }
public virtual int GetLimitMotorInfo2(RotationalLimitMotor limot, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 linVelA, ref IndexedVector3 linVelB, ref IndexedVector3 angVelA, ref IndexedVector3 angVelB, ConstraintInfo2 info, int row, ref IndexedVector3 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 //float* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; //float* 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) { IndexedVector3 tmpA = IndexedVector3.Zero, tmpB = IndexedVector3.Zero, relA = IndexedVector3.Zero, relB = IndexedVector3.Zero; // get vector from bodyB to frameB in WCS relB = m_calculatedTransformB._origin - transB._origin; // get its projection to constraint axis IndexedVector3 projB = ax1 * IndexedVector3.Dot(relB, ax1); // get vector directed from bodyB to constraint axis (and orthogonal to it) IndexedVector3 orthoB = relB - projB; // same for bodyA relA = m_calculatedTransformA._origin - transA._origin; IndexedVector3 projA = ax1 * IndexedVector3.Dot(relA, ax1); IndexedVector3 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 IndexedVector3 totalDist = projA + ax1 * desiredOffs - projB; // get offset vectors relA and relB relA = orthoA + totalDist * m_factA; relB = orthoB - totalDist * m_factB; tmpA = IndexedVector3.Cross(relA, ax1); tmpB = IndexedVector3.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 { IndexedVector3 ltd; // Linear Torque Decoupling vector IndexedVector3 c = m_calculatedTransformB._origin - transA._origin; ltd = IndexedVector3.Cross(c, ax1); info.m_solverConstraints[row].m_relpos1CrossNormal = ltd; MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_relpos1CrossNormal); c = m_calculatedTransformB._origin - transB._origin; ltd = -IndexedVector3.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 = IndexedVector3.Dot(angVelA, ax1); vel -= IndexedVector3.Dot(angVelB, ax1); } else { vel = IndexedVector3.Dot(linVelA, ax1); vel -= IndexedVector3.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; }