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);
}
}
