public int GetLimitMotorInfo2(RotationalLimitMotor limitMotor, Matrix transA, Matrix transB, Vector3 linVelA, Vector3 linVelB, Vector3 angVelA, Vector3 angVelB, ConstraintInfo2 info, int row, ref Vector3 ax1, int rotational, int rotAllowed) { return(btGeneric6DofConstraint_get_limit_motor_info22(_native, limitMotor._native, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB, info._native, row, ref ax1, rotational, rotAllowed)); }
public override void getInfo2(ConstraintInfo2 info) { // this will be called by constraint solver at the constraint setup stage // set current motor parameters internalUpdateSprings(info); // do the rest of job for constraint setup base.getInfo2(info); }
public void GetInfo2NonVirtual(ConstraintInfo2 info, Matrix transA, Matrix transB, Vector3 angVelA, Vector3 angVelB) { btHingeConstraint_getInfo2NonVirtual(_native, info._native, ref transA, ref transB, ref angVelA, ref angVelB); }
public void GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, Matrix transA, Matrix transB, Vector3 angVelA, Vector3 angVelB) { btHingeConstraint_getInfo2InternalUsingFrameOffset(_native, info._native, ref transA, ref transB, ref angVelA, ref angVelB); }
public void GetInfo2NonVirtual(ConstraintInfo2 info, Matrix transA, Matrix transB, Vector3 linVelA, Vector3 linVelB, float rbAinvMass, float rbBinvMass) { btSliderConstraint_getInfo2NonVirtual(_native, info._native, ref transA, ref transB, ref linVelA, ref linVelB, rbAinvMass, rbBinvMass); }
protected int setLinearLimits(ConstraintInfo2 info, int row, btTransform transA, btTransform transB, btVector3 linVelA, btVector3 linVelB, btVector3 angVelA, btVector3 angVelB) { // int row = 0; //solve linear limits limot.Reset(); 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 = 0f; limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; btVector3 axis;// = m_calculatedTransformA.Basis.getColumn(i); m_calculatedTransformA.Basis.getColumn(i, out axis); bt6DofFlags flags = (bt6DofFlags)((int)m_flags >> (i * (int)bt6DofFlags.BT_6DOF_FLAGS_AXIS_SHIFT)); //limot.m_normalCFM = (flags & bt6DofFlags.BT_6DOF_FLAGS_CFM_NORM) != 0 ? m_linearLimits.m_normalCFM[i] : info.cfm[0]; //limot.m_stopCFM = (flags & bt6DofFlags.BT_6DOF_FLAGS_CFM_STOP) != 0 ? m_linearLimits.m_stopCFM[i] : info.cfm[0]; limot.m_normalCFM = (flags & bt6DofFlags.BT_6DOF_FLAGS_CFM_NORM) != 0 ? m_linearLimits.m_normalCFM[i] : info.Constraints[info.CurrentRow].m_cfm; limot.m_stopCFM = (flags & bt6DofFlags.BT_6DOF_FLAGS_CFM_STOP) != 0 ? m_linearLimits.m_stopCFM[i] : info.Constraints[info.CurrentRow].m_cfm; limot.m_stopERP = (flags & bt6DofFlags.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 += get_limit_motor_info2(limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, ref axis, false, rotAllowed); } else { row += get_limit_motor_info2(limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, ref axis, false); } } } return row; }
public int GetLimitMotorInfo2(RotationalLimitMotor limitMotor, Matrix transA, Matrix transB, Vector3 linVelA, Vector3 linVelB, Vector3 angVelA, Vector3 angVelB, ConstraintInfo2 info, int row, out Vector3 ax1, int rotational, int rotAllowed) { return btGeneric6DofConstraint_get_limit_motor_info22(_native, limitMotor._native, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB, info._native, row, out ax1, rotational, rotAllowed); }
public void GetInfo2NonVirtual(ConstraintInfo2 info, Matrix transA, Matrix transB, Matrix invInertiaWorldA, Matrix invInertiaWorldB) { btConeTwistConstraint_getInfo2NonVirtual(Native, info._native, ref transA, ref transB, ref invInertiaWorldA, ref invInertiaWorldB); }
public void GetInfo2NonVirtual(ConstraintInfo2 info, Matrix body0Trans, Matrix body1Trans) { btPoint2PointConstraint_getInfo2NonVirtual(_native, info._native, ref body0Trans, ref body1Trans); }
public void GetInfo2NonVirtual(ConstraintInfo2 info, Matrix body0Trans, Matrix body1Trans) { btPoint2PointConstraint_getInfo2NonVirtual(_native, info._native, ref body0Trans, ref body1Trans); }
protected void internalUpdateSprings(ConstraintInfo2 info) { // it is assumed that calculateTransforms() have been called before this call int i; btVector3 relVel = m_rbB.LinearVelocity - m_rbA.LinearVelocity; for (i = 0; i < 3; i++) { if (m_springEnabled[i]) { // get current position of constraint float currPos = m_calculatedLinearDiff[i]; // calculate difference float delta = currPos - m_equilibriumPoint[i]; // spring force is (delta * m_stiffness) according to Hooke's Law float force = delta * m_springStiffness[i]; float velFactor = info.fps * m_springDamping[i] / info.m_numIterations; m_linearLimits.m_targetVelocity[i] = velFactor * force; m_linearLimits.m_maxMotorForce[i] = (float)Math.Abs(force) / info.fps; } } for (i = 0; i < 3; i++) { if (m_springEnabled[i + 3]) { // get current position of constraint float currPos = m_calculatedAxisAngleDiff[i]; // calculate difference float delta = currPos - m_equilibriumPoint[i + 3]; // spring force is (-delta * m_stiffness) according to Hooke's Law float force = -delta * m_springStiffness[i + 3]; float velFactor = info.fps * m_springDamping[i + 3] / info.m_numIterations; m_angularLimits[i].m_targetVelocity = velFactor * force; m_angularLimits[i].m_maxMotorForce = (float)Math.Abs(force) / info.fps; } } }
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; }
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); }
protected int setAngularLimits(ConstraintInfo2 info, int row_offset, btTransform transA, btTransform transB, btVector3 linVelA, btVector3 linVelB, btVector3 angVelA, btVector3 angVelB) { Generic6DofConstraint d6constraint = this; int row = row_offset; //solve angular limits for (int i = 0; i < 3; i++) { if (d6constraint.getRotationalLimitMotor(i).needApplyTorques()) { btVector3 axis = d6constraint.getAxis(i); bt6DofFlags flags = (bt6DofFlags)((int)m_flags >> ((i + 3) * (int)bt6DofFlags.BT_6DOF_FLAGS_AXIS_SHIFT)); if ((flags & bt6DofFlags.BT_6DOF_FLAGS_CFM_NORM) == 0) { m_angularLimits[i].m_normalCFM = info.Constraints[info.CurrentRow].m_cfm; } if ((flags & bt6DofFlags.BT_6DOF_FLAGS_CFM_STOP) == 0) { m_angularLimits[i].m_stopCFM = info.Constraints[info.CurrentRow].m_cfm; } if ((flags & bt6DofFlags.BT_6DOF_FLAGS_ERP_STOP) == 0) { m_angularLimits[i].m_stopERP = info.erp; } row += get_limit_motor_info2(d6constraint.getRotationalLimitMotor(i), transA, transB, linVelA, linVelB, angVelA, angVelB, info, row,ref axis, true); } } return row; }
public void GetInfo2(ConstraintInfo2 info) { btTypedConstraint_getInfo2(_native, info._native); }
public void GetInfo2Internal(ConstraintInfo2 info, Matrix transA, Matrix transB, Vector3 angVelA, Vector3 angVelB) { btHingeConstraint_getInfo2Internal(Native, info.Native, ref transA, ref transB, ref angVelA, ref angVelB); }
public void GetInfo2NonVirtual(ConstraintInfo2 info, Matrix transA, Matrix transB, Matrix invInertiaWorldA, Matrix invInertiaWorldB) { btConeTwistConstraint_getInfo2NonVirtual(_native, info._native, ref transA, ref transB, ref invInertiaWorldA, ref invInertiaWorldB); }
public void GetInfo2NonVirtual(ConstraintInfo2 info, Matrix transA, Matrix transB, Vector3 linVelA, Vector3 linVelB, float rbAinvMass, float rbBinvMass) { btSliderConstraint_getInfo2NonVirtual(_native, info._native, ref transA, ref transB, ref linVelA, ref linVelB, rbAinvMass, rbBinvMass); }
public void GetInfo2NonVirtual(ConstraintInfo2 info, Matrix transA, Matrix transB, Vector3 linVelA, Vector3 linVelB, Vector3 angVelA, Vector3 angVelB) { btGeneric6DofConstraint_getInfo2NonVirtual(_native, info._native, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB); }
public void GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, Matrix transA, Matrix transB, Vector3 angVelA, Vector3 angVelB) { btHingeConstraint_getInfo2InternalUsingFrameOffset(_native, info._native, ref transA, ref transB, ref angVelA, ref angVelB); }
public void GetInfo2(ConstraintInfo2 info) { btTypedConstraint_getInfo2(_native, info._native); }
public void GetInfo2NonVirtual(ConstraintInfo2 info, Matrix transA, Matrix transB, Vector3 angVelA, Vector3 angVelB) { btHingeConstraint_getInfo2NonVirtual(_native, info._native, ref transA, ref transB, ref angVelA, ref angVelB); }
public void GetInfo2NonVirtual(ConstraintInfo2 info, Matrix transA, Matrix transB, Vector3 linVelA, Vector3 linVelB, Vector3 angVelA, Vector3 angVelB) { btGeneric6DofConstraint_getInfo2NonVirtual(_native, info._native, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB); }
void getInfo2NonVirtual(ConstraintInfo2 info, btTransform transA, btTransform transB, btVector3 linVelA, btVector3 linVelB, btVector3 angVelA, btVector3 angVelB) { Debug.Assert(!m_useSolveConstraintObsolete); //prepare constraint calculateTransforms(transA, transB); if (m_useOffsetForConstraintFrame) { // for stability better to solve angular limits first int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB); setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB); } else { // leave old version for compatibility int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB); setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB); } }