bool SliderJointSW::setup(float p_step)
{
//calculate transforms
m_calculatedTransformA = A->get_transform() * m_frameInA;
m_calculatedTransformB = B->get_transform() * m_frameInB;
m_realPivotAInW = m_calculatedTransformA.origin;
m_realPivotBInW = m_calculatedTransformB.origin;
m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X
m_delta = m_realPivotBInW - m_realPivotAInW;
m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
m_relPosA = m_projPivotInW - A->get_transform().origin;
m_relPosB = m_realPivotBInW - B->get_transform().origin;
Vector3 normalWorld;
int i;
//linear part
for(i = 0; i < 3; i++)
{
normalWorld = m_calculatedTransformA.basis.get_axis(i);
memnew_placement(&m_jacLin[i], JacobianEntrySW(
A->get_transform().basis.transposed(),
B->get_transform().basis.transposed(),
m_relPosA,
m_relPosB,
normalWorld,
A->get_inv_inertia(),
A->get_inv_mass(),
B->get_inv_inertia(),
B->get_inv_mass()
));
m_jacLinDiagABInv[i] = real_t(1.) / m_jacLin[i].getDiagonal();
m_depth[i] = m_delta.dot(normalWorld);
}
testLinLimits();
// angular part
for(i = 0; i < 3; i++)
{
normalWorld = m_calculatedTransformA.basis.get_axis(i);
memnew_placement(&m_jacAng[i], JacobianEntrySW(
normalWorld,
A->get_transform().basis.transposed(),
B->get_transform().basis.transposed(),
A->get_inv_inertia(),
B->get_inv_inertia()
));
}
testAngLimits();
Vector3 axisA = m_calculatedTransformA.basis.get_axis(0);
m_kAngle = real_t(1.0 )/ (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA));
// clear accumulator for motors
m_accumulatedLinMotorImpulse = real_t(0.0);
m_accumulatedAngMotorImpulse = real_t(0.0);
return true;
} // SliderJointSW::buildJacobianInt()
void Generic6DOFJointSW::buildAngularJacobian(
JacobianEntrySW & jacAngular,const Vector3 & jointAxisW)
{
memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW,
A->get_transform().basis.transposed(),
B->get_transform().basis.transposed(),
A->get_inv_inertia(),
B->get_inv_inertia()));
}
void Generic6DOFJointSW::buildLinearJacobian(
JacobianEntrySW & jacLinear,const Vector3 & normalWorld,
const Vector3 & pivotAInW,const Vector3 & pivotBInW)
{
memnew_placement(&jacLinear, JacobianEntrySW(
A->get_transform().basis.transposed(),
B->get_transform().basis.transposed(),
pivotAInW - A->get_transform().origin,
pivotBInW - B->get_transform().origin,
normalWorld,
A->get_inv_inertia(),
A->get_inv_mass(),
B->get_inv_inertia(),
B->get_inv_mass()));
}
bool PinJointSW::setup(float p_step) {
m_appliedImpulse = real_t(0.);
Vector3 normal(0, 0, 0);
for (int i = 0; i < 3; i++) {
normal[i] = 1;
memnew_placement(&m_jac[i], JacobianEntrySW(
A->get_transform().basis.transposed(),
B->get_transform().basis.transposed(),
A->get_transform().xform(m_pivotInA) - A->get_transform().origin,
B->get_transform().xform(m_pivotInB) - B->get_transform().origin,
normal,
A->get_inv_inertia(),
A->get_inv_mass(),
B->get_inv_inertia(),
B->get_inv_mass()));
normal[i] = 0;
}
return true;
}
bool ConeTwistJointSW::setup(real_t p_timestep) {
m_appliedImpulse = real_t(0.);
//set bias, sign, clear accumulator
m_swingCorrection = real_t(0.);
m_twistLimitSign = real_t(0.);
m_solveTwistLimit = false;
m_solveSwingLimit = false;
m_accTwistLimitImpulse = real_t(0.);
m_accSwingLimitImpulse = real_t(0.);
if (!m_angularOnly) {
Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
Vector3 relPos = pivotBInW - pivotAInW;
Vector3 normal[3];
if (relPos.length_squared() > CMP_EPSILON) {
normal[0] = relPos.normalized();
} else {
normal[0] = Vector3(real_t(1.0), 0, 0);
}
plane_space(normal[0], normal[1], normal[2]);
for (int i = 0; i < 3; i++) {
memnew_placement(&m_jac[i], JacobianEntrySW(
A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(),
pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
normal[i],
A->get_inv_inertia(),
A->get_inv_mass(),
B->get_inv_inertia(),
B->get_inv_mass()));
}
}
Vector3 b1Axis1, b1Axis2, b1Axis3;
Vector3 b2Axis1, b2Axis2;
b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(0));
b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(0));
real_t swing1 = real_t(0.), swing2 = real_t(0.);
real_t swx = real_t(0.), swy = real_t(0.);
real_t thresh = real_t(10.);
real_t fact;
// Get Frame into world space
if (m_swingSpan1 >= real_t(0.05f)) {
b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(1));
//swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis2);
swing1 = atan2fast(swy, swx);
fact = (swy * swy + swx * swx) * thresh * thresh;
fact = fact / (fact + real_t(1.0));
swing1 *= fact;
}
if (m_swingSpan2 >= real_t(0.05f)) {
b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(2));
//swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis3);
swing2 = atan2fast(swy, swx);
fact = (swy * swy + swx * swx) * thresh * thresh;
fact = fact / (fact + real_t(1.0));
swing2 *= fact;
}
real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1);
real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2);
real_t EllipseAngle = Math::abs(swing1 * swing1) * RMaxAngle1Sq + Math::abs(swing2 * swing2) * RMaxAngle2Sq;
if (EllipseAngle > 1.0f) {
m_swingCorrection = EllipseAngle - 1.0f;
m_solveSwingLimit = true;
// Calculate necessary axis & factors
m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3));
m_swingAxis.normalize();
real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
m_swingAxis *= swingAxisSign;
m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) +
B->compute_angular_impulse_denominator(m_swingAxis));
}
// Twist limits
if (m_twistSpan >= real_t(0.)) {
Vector3 b2Axis2 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(1));
Quat rotationArc = Quat(b2Axis1, b1Axis1);
Vector3 TwistRef = rotationArc.xform(b2Axis2);
real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));
real_t lockedFreeFactor = (m_twistSpan > real_t(0.05f)) ? m_limitSoftness : real_t(0.);
if (twist <= -m_twistSpan * lockedFreeFactor) {
m_twistCorrection = -(twist + m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
m_twistAxis *= -1.0f;
m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) +
B->compute_angular_impulse_denominator(m_twistAxis));
} else if (twist > m_twistSpan * lockedFreeFactor) {
m_twistCorrection = (twist - m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) +
B->compute_angular_impulse_denominator(m_twistAxis));
}
}
return true;
}
bool HingeJointSW::setup(float p_step) {
m_appliedImpulse = real_t(0.);
if (!m_angularOnly) {
Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
Vector3 relPos = pivotBInW - pivotAInW;
Vector3 normal[3];
if (relPos.length_squared() > CMP_EPSILON) {
normal[0] = relPos.normalized();
} else {
normal[0] = Vector3(real_t(1.0), 0, 0);
}
plane_space(normal[0], normal[1], normal[2]);
for (int i = 0; i < 3; i++) {
memnew_placement(&m_jac[i], JacobianEntrySW(
A->get_transform().basis.transposed(),
B->get_transform().basis.transposed(),
pivotAInW - A->get_transform().origin,
pivotBInW - B->get_transform().origin,
normal[i],
A->get_inv_inertia(),
A->get_inv_mass(),
B->get_inv_inertia(),
B->get_inv_mass()));
}
}
//calculate two perpendicular jointAxis, orthogonal to hingeAxis
//these two jointAxis require equal angular velocities for both bodies
//this is unused for now, it's a todo
Vector3 jointAxis0local;
Vector3 jointAxis1local;
plane_space(m_rbAFrame.basis.get_axis(2), jointAxis0local, jointAxis1local);
A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2));
Vector3 jointAxis0 = A->get_transform().basis.xform(jointAxis0local);
Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local);
Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2));
memnew_placement(&m_jacAng[0], JacobianEntrySW(jointAxis0,
A->get_transform().basis.transposed(),
B->get_transform().basis.transposed(),
A->get_inv_inertia(),
B->get_inv_inertia()));
memnew_placement(&m_jacAng[1], JacobianEntrySW(jointAxis1,
A->get_transform().basis.transposed(),
B->get_transform().basis.transposed(),
A->get_inv_inertia(),
B->get_inv_inertia()));
memnew_placement(&m_jacAng[2], JacobianEntrySW(hingeAxisWorld,
A->get_transform().basis.transposed(),
B->get_transform().basis.transposed(),
A->get_inv_inertia(),
B->get_inv_inertia()));
// Compute limit information
real_t hingeAngle = get_hinge_angle();
// print_line("angle: "+rtos(hingeAngle));
//set bias, sign, clear accumulator
m_correction = real_t(0.);
m_limitSign = real_t(0.);
m_solveLimit = false;
m_accLimitImpulse = real_t(0.);
/*if (m_useLimit) {
print_line("low: "+rtos(m_lowerLimit));
print_line("hi: "+rtos(m_upperLimit));
}*/
// if (m_lowerLimit < m_upperLimit)
if (m_useLimit && m_lowerLimit <= m_upperLimit) {
// if (hingeAngle <= m_lowerLimit*m_limitSoftness)
if (hingeAngle <= m_lowerLimit) {
m_correction = (m_lowerLimit - hingeAngle);
m_limitSign = 1.0f;
m_solveLimit = true;
}
// else if (hingeAngle >= m_upperLimit*m_limitSoftness)
else if (hingeAngle >= m_upperLimit) {
m_correction = m_upperLimit - hingeAngle;
m_limitSign = -1.0f;
m_solveLimit = true;
}
}
//Compute K = J*W*J' for hinge axis
Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2));
m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) +
B->compute_angular_impulse_denominator(axisA));
return true;
}
