public float computeAngularImpulseDenominator(btVector3 axis)
{
btVector3 vec;// = axis * InvInertiaTensorWorld;
btMatrix3x3.Multiply(ref axis, ref InvInertiaTensorWorld, out vec);
return axis.dot(vec);
}
public float computeImpulseDenominator(btVector3 pos, btVector3 normal)
{
btVector3 r0 = pos - CenterOfMassPosition;
btVector3 c0 = (r0).cross(normal);
btVector3 vec;
#region btVector3 vec = (c0 * InvInertiaTensorWorld).cross(r0);
{
btVector3 temp;
btMatrix3x3.Multiply(ref c0, ref InvInertiaTensorWorld, out temp);
vec = temp.cross(r0);
}
#endregion
return m_inverseMass + normal.dot(vec);
}
static void segmentsClosestPoints(
out btVector3 ptsVector,
out btVector3 offsetA,
out btVector3 offsetB,
out float tA, out float tB,
ref btVector3 translation,
ref btVector3 dirA, float hlenA,
ref btVector3 dirB, float hlenB)
{
// compute the parameters of the closest points on each line segment
float dirA_dot_dirB = dirA.dot(dirB);
float dirA_dot_trans = dirA.dot(translation);
float dirB_dot_trans = dirB.dot(translation);
float denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
if (denom == 0.0f)
{
tA = 0.0f;
}
else
{
tA = (dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB) / denom;
if (tA < -hlenA)
tA = -hlenA;
else if (tA > hlenA)
tA = hlenA;
}
tB = tA * dirA_dot_dirB - dirB_dot_trans;
if (tB < -hlenB)
{
tB = -hlenB;
tA = tB * dirA_dot_dirB + dirA_dot_trans;
if (tA < -hlenA)
tA = -hlenA;
else if (tA > hlenA)
tA = hlenA;
}
else if (tB > hlenB)
{
tB = hlenB;
tA = tB * dirA_dot_dirB + dirA_dot_trans;
if (tA < -hlenA)
tA = -hlenA;
else if (tA > hlenA)
tA = hlenA;
}
// compute the closest points relative to segment centers.
//offsetA = dirA * tA;
btVector3.Multiply(ref dirA, tA, out offsetA);
//offsetB = dirB * tB;
btVector3.Multiply(ref dirB, tB, out offsetB);
#region ptsVector = translation - offsetA + offsetB;
{
btVector3 temp;
btVector3.Subtract(ref translation, ref offsetA, out temp);
btVector3.Add(ref temp, ref offsetB, out ptsVector);
}
#endregion
}
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 static float dot(ref btVector3 v1, ref btVector3 v2)
{
return v1.dot(ref v2);
}
public static float dot(btVector3 v1, btVector3 v2)
{
return v1.dot(v2);
}
