btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const
{
// use full newton-euler equations. common practice to drop the wxIw term. want it for better tumbling behavior.
// calculate using implicit euler step so it's stable.
const btVector3 inertiaLocal = getLocalInertia();
const btVector3 w0 = getAngularVelocity();
btMatrix3x3 I;
I = m_worldTransform.getBasis().scaled(inertiaLocal) *
m_worldTransform.getBasis().transpose();
// use newtons method to find implicit solution for new angular velocity (w')
// f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0
// df/dw' = I + 1xIw'*step + w'xI*step
btVector3 w1 = w0;
// one step of newton's method
{
const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I);
const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I);
btVector3 dw;
dw = dfw.solve33(fw);
//const btMatrix3x3 dfw_inv = dfw.inverse();
//dw = dfw_inv*fw;
w1 -= dw;
}
btVector3 gf = (w1 - w0);
return gf;
}
static btMatrix3x3 Transpose(btMatrix3x3 &in)
{
btVector3 row0 = in.getRow(0);
btVector3 row1 = in.getRow(1);
btVector3 row2 = in.getRow(2);
btVector3 col0 = btAssign128(row0.x(), row1.x(), row2.x(), 0);
btVector3 col1 = btAssign128(row0.y(), row1.y(), row2.y(), 0);
btVector3 col2 = btAssign128(row0.z(), row1.z(), row2.z(), 0);
return btMatrix3x3(col0, col1, col2);
}
Matrix3d toMatrix3d(const btMatrix3x3& basis)
{
Matrix3d rotation;
btVector3 col0 = basis.getColumn(0);
btVector3 col1 = basis.getColumn(1);
btVector3 col2 = basis.getColumn(2);
rotation.col(0) = toVector3d(col0);
rotation.col(1) = toVector3d(col1);
rotation.col(2) = toVector3d(col2);
return rotation;
}
void virtuose::Matrix3ToArray(btMatrix3x3 m, float *to) {
to[0] = m.getRow(0).getX();
to[1] = m.getRow(0).getZ();
to[2] = m.getRow(0).getY();
to[3] = m.getRow(2).getX();
to[4] = m.getRow(2).getZ();
to[5] = m.getRow(2).getY();
to[6] = m.getRow(1).getZ();
to[7] = m.getRow(1).getX();
to[8] = m.getRow(1).getY();
}
/** Set the orientation (forward and up vectors) to the
* provided rotation matrix
* @param matrix matrix to use to set new orientation
* @note this method is provided to ease performance interactions with
* the physics library
*/
void Position::setRowMajorRotationMatrix(const btMatrix3x3& matrix)
{
btVector3 upV = matrix.getRow(1);
btVector3 forwardV = matrix.getRow(2);
// fill in y axis, second row
up.set(upV.getX(), upV.getY(), upV.getZ());
// fill in z axis, thrid row
forward.set(forwardV.getX(), forwardV.getY(), forwardV.getZ());
}
void ConvertRotationToHL(const btMatrix3x3& matrix, QAngle& hl) {
btQuaternion quat;
matrix.getRotation(quat);
Quaternion q(quat.getX(), -quat.getZ(), quat.getY(), quat.getW());
RadianEuler radian(q);
hl = radian.ToQAngle();
}
/** Get the rotation matrix into a 3x3 matrix
* @param matrix out parameter to place matrix data into
* @note this method is provided to ease performance interactions with
* the physics library
*/
void Position::getRowMajorRotationMatrix(btMatrix3x3& matrix)
{
// get local x axis
Vector3f xAxis;
xAxis.crossProduct(up, forward);
matrix.setValue (
// fill in x axis, first column
xAxis.getX(),
xAxis.getY(),
xAxis.getZ(),
// fill in y axis, second column
up.getX(),
up.getY(),
up.getZ(),
// fill in z axis, thrid column
forward.getX(),
forward.getY(),
forward.getZ()
);
}
void btSetCrossMatrixMinus(btMatrix3x3& res, const btVector3& a)
{
const btScalar a_0 = a[0], a_1 = a[1], a_2 = a[2];
res.setValue(0, +a_2, -a_1,
-a_2, 0, +a_0,
+a_1, -a_0, 0);
}
QDomElement SimDomElement::basisToNode(QDomDocument &doc, const btMatrix3x3 mx)
{
QDomElement matrix = doc.createElement( "matrix" );
for(int i=0;i<3;i++)
matrix.appendChild(vectorToNode(doc, mx.getRow(i)));
return matrix;
}
static int operator!= ( const btMatrix3x3 &a, const btMatrix3x3 &b )
{
int i;
btVector3 av3, bv3;
for(i=0; i<3; i++)
{
av3 = a.getRow(i);
bv3 = b.getRow(i);
if( fabs(av3.m_floats[0] - bv3.m_floats[0]) +
fabs(av3.m_floats[1] - bv3.m_floats[1]) +
fabs(av3.m_floats[2] - bv3.m_floats[2]) > FLT_EPSILON * 4)
return 1;
}
return 0;
}
ork::CMatrix3 btbasistoorkmtx3( const btMatrix3x3& mtx )
{ ork::CMatrix3 rval;
for( int i=0; i<3; i++ )
{ const btVector3& vec = mtx.getColumn(i);
rval.SetElemXY(i,0,float(vec.x()));
rval.SetElemXY(i,1,float(vec.y()));
rval.SetElemXY(i,2,float(vec.z()));
}
return rval;
}
void convertToBtTransform(const Vector3d& start_pos, const Vector3d& end_pos, btVector3& origin, btMatrix3x3& basis)
{
Matrix3d rotation;
rotation_from_tangent((start_pos - end_pos).normalized(), rotation);
basis.setValue(rotation(0,0), rotation(0,1), rotation(0,2),
rotation(1,0), rotation(1,1), rotation(1,2),
rotation(2,0), rotation(2,1), rotation(2,2));
Vector3d mid_point = (start_pos + end_pos)/2.0;
origin.setValue(mid_point(0), mid_point(1), mid_point(2));
}
osg::Matrix
osgbCollision::asOsgMatrix( const btMatrix3x3& m )
{
btScalar f[ 9 ];
m.getOpenGLSubMatrix( f );
return( osg::Matrix(
f[0], f[1], f[2], 0.,
f[3], f[4], f[5], 0.,
f[6], f[7], f[8], 0.,
0., 0., 0., 1. ) );
}
void btMatrix3_to_Matrix3(JNIEnv * const &jenv, jobject &target, const btMatrix3x3 &source)
{
matrix3_ensurefields(jenv, target);
jfloatArray valArray = (jfloatArray) jenv->GetObjectField(target, matrix3_val);
jfloat * elements = jenv->GetFloatArrayElements(valArray, NULL);
// Convert to column-major
elements[0] = (jfloat) source.getColumn(0).getX();
elements[1] = (jfloat) source.getColumn(0).getY();
elements[2] = (jfloat) source.getColumn(0).getZ();
elements[3] = (jfloat) source.getColumn(1).getX();
elements[4] = (jfloat) source.getColumn(1).getY();
elements[5] = (jfloat) source.getColumn(1).getZ();
elements[6] = (jfloat) source.getColumn(2).getX();
elements[7] = (jfloat) source.getColumn(2).getY();
elements[8] = (jfloat) source.getColumn(2).getZ();
jenv->ReleaseFloatArrayElements(valArray, elements, 0);
jenv->DeleteLocalRef(valArray);
}
static btMatrix3x3 M3x3mulM1M2_ref( const btMatrix3x3 &m1, const btMatrix3x3 &m2 )
{
return btMatrix3x3(
m2.tdotx(m1[0]), m2.tdoty(m1[0]), m2.tdotz(m1[0]),
m2.tdotx(m1[1]), m2.tdoty(m1[1]), m2.tdotz(m1[1]),
m2.tdotx(m1[2]), m2.tdoty(m1[2]), m2.tdotz(m1[2]));
}
static int operator!= ( const btMatrix3x3 &a, const btMatrix3x3 &b )
{
if( a.getRow(0) != b.getRow(0) )
return 1;
if( a.getRow(1) != b.getRow(1) )
return 1;
if( a.getRow(2) != b.getRow(2) )
return 1;
return 0;
}
void Matrix3_to_btMatrix3(JNIEnv * const &jenv, btMatrix3x3 &target, jobject &source)
{
matrix3_ensurefields(jenv, source);
jfloatArray valArray = (jfloatArray) jenv->GetObjectField(source, matrix3_val);
jfloat * elements = jenv->GetFloatArrayElements(valArray, NULL);
// Convert to column-major
target.setValue(
elements[0], elements[3], elements[6],
elements[1], elements[4], elements[7],
elements[2], elements[5], elements[8]);
jenv->ReleaseFloatArrayElements(valArray, elements, JNI_ABORT);
jenv->DeleteLocalRef(valArray);
}
static btMatrix3x3 M3x3setRot_ref( btMatrix3x3 &m, const btQuaternion &q )
{
btScalar d = q.length2();
btScalar s = btScalar(2.0) / d;
btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
m.setValue(
btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
return m;
}
unsigned int btPolarDecomposition::decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const
{
// Use the 'u' and 'h' matrices for intermediate calculations
u = a;
h = a.inverse();
for (unsigned int i = 0; i < m_maxIterations; ++i)
{
const btScalar h_1 = p1_norm(h);
const btScalar h_inf = pinf_norm(h);
const btScalar u_1 = p1_norm(u);
const btScalar u_inf = pinf_norm(u);
const btScalar h_norm = h_1 * h_inf;
const btScalar u_norm = u_1 * u_inf;
// The matrix is effectively singular so we cannot invert it
if (btFuzzyZero(h_norm) || btFuzzyZero(u_norm))
break;
const btScalar gamma = btPow(h_norm / u_norm, 0.25f);
const btScalar inv_gamma = 1.0 / gamma;
// Determine the delta to 'u'
const btMatrix3x3 delta = (u * (gamma - 2.0) + h.transpose() * inv_gamma) * 0.5;
// Update the matrices
u += delta;
h = u.inverse();
// Check for convergence
if (p1_norm(delta) <= m_tolerance * u_1)
{
h = u.transpose() * a;
h = (h + h.transpose()) * 0.5;
return i;
}
}
// The algorithm has failed to converge to the specified tolerance, but we
// want to make sure that the matrices returned are in the right form.
h = u.transpose() * a;
h = (h + h.transpose()) * 0.5;
return m_maxIterations;
}
static int operator!= ( const btMatrix3x3 &a, const btMatrix3x3 &b )
{
if( a.getRow(0) != b.getRow(0) )
{
if (!fuzzyEqualSlow(a.getRow(0),b.getRow(0)))
{
return 1;
}
}
if( a.getRow(1) != b.getRow(1) )
{
if( !fuzzyEqualSlow(a.getRow(1),b.getRow(1)) )
return 1;
}
if( a.getRow(2) != b.getRow(2) )
{
if( !fuzzyEqualSlow(a.getRow(2),b.getRow(2)) )
{
return 1;
}
}
return 0;
}
void btMultiBody::stepVelocities(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m)
{
// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
// and the base linear & angular accelerations.
// We apply damping forces in this routine as well as any external forces specified by the
// caller (via addBaseForce etc).
// output should point to an array of 6 + num_links reals.
// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
// num_links joint acceleration values.
int num_links = getNumLinks();
const btScalar DAMPING_K1_LINEAR = m_linearDamping;
const btScalar DAMPING_K2_LINEAR = m_linearDamping;
const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
btVector3 base_vel = getBaseVel();
btVector3 base_omega = getBaseOmega();
// Temporary matrices/vectors -- use scratch space from caller
// so that we don't have to keep reallocating every frame
scratch_r.resize(2*num_links + 6);
scratch_v.resize(8*num_links + 6);
scratch_m.resize(4*num_links + 4);
btScalar * r_ptr = &scratch_r[0];
btScalar * output = &scratch_r[num_links]; // "output" holds the q_double_dot results
btVector3 * v_ptr = &scratch_v[0];
// vhat_i (top = angular, bottom = linear part)
btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
// zhat_i^A
btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
// chat_i (note NOT defined for the base)
btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
// top left, top right and bottom left blocks of Ihat_i^A.
// bottom right block = transpose of top left block and is not stored.
// Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently.
btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
// Cached 3x3 rotation matrices from parent frame to this frame.
btMatrix3x3 * rot_from_parent = &matrix_buf[0];
btMatrix3x3 * rot_from_world = &scratch_m[0];
// hhat_i, ahat_i
// hhat is NOT stored for the base (but ahat is)
btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0;
btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0;
btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
// Y_i, D_i
btScalar * Y = r_ptr; r_ptr += num_links;
btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0;
// ptr to the joint accel part of the output
btScalar * joint_accel = output + 6;
// Start of the algorithm proper.
// First 'upward' loop.
// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
rot_from_parent[0] = btMatrix3x3(base_quat);
vel_top_angular[0] = rot_from_parent[0] * base_omega;
vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
if (fixed_base) {
zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
} else {
zero_acc_top_angular[0] = - (rot_from_parent[0] * (base_force
- base_mass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
zero_acc_bottom_linear[0] =
- (rot_from_parent[0] * base_torque);
if (m_useGyroTerm)
zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( base_inertia * vel_top_angular[0] );
zero_acc_bottom_linear[0] += base_inertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
}
inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
inertia_top_right[0].setValue(base_mass, 0, 0,
0, base_mass, 0,
0, 0, base_mass);
inertia_bottom_left[0].setValue(base_inertia[0], 0, 0,
0, base_inertia[1], 0,
0, 0, base_inertia[2]);
rot_from_world[0] = rot_from_parent[0];
for (int i = 0; i < num_links; ++i) {
const int parent = links[i].parent;
rot_from_parent[i+1] = btMatrix3x3(links[i].cached_rot_parent_to_this);
rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
// vhat_i = i_xhat_p(i) * vhat_p(i)
SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
vel_top_angular[parent+1], vel_bottom_linear[parent+1],
vel_top_angular[i+1], vel_bottom_linear[i+1]);
// we can now calculate chat_i
// remember vhat_i is really vhat_p(i) (but in current frame) at this point
coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(links[i].cached_r_vector))
+ 2 * vel_top_angular[i+1].cross(links[i].axis_bottom) * getJointVel(i);
if (links[i].is_revolute) {
coriolis_top_angular[i] = vel_top_angular[i+1].cross(links[i].axis_top) * getJointVel(i);
coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * links[i].axis_top.cross(links[i].axis_bottom);
} else {
coriolis_top_angular[i] = btVector3(0,0,0);
}
// now set vhat_i to its true value by doing
// vhat_i += qidot * shat_i
vel_top_angular[i+1] += getJointVel(i) * links[i].axis_top;
vel_bottom_linear[i+1] += getJointVel(i) * links[i].axis_bottom;
// calculate zhat_i^A
zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (links[i].applied_force));
zero_acc_top_angular[i+1] += links[i].mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
zero_acc_bottom_linear[i+1] =
- (rot_from_world[i+1] * links[i].applied_torque);
if (m_useGyroTerm)
{
zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( links[i].inertia * vel_top_angular[i+1] );
}
zero_acc_bottom_linear[i+1] += links[i].inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
// calculate Ihat_i^A
inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
inertia_top_right[i+1].setValue(links[i].mass, 0, 0,
0, links[i].mass, 0,
0, 0, links[i].mass);
inertia_bottom_left[i+1].setValue(links[i].inertia[0], 0, 0,
0, links[i].inertia[1], 0,
0, 0, links[i].inertia[2]);
}
// 'Downward' loop.
// (part of TreeForwardDynamics in Mirtich.)
for (int i = num_links - 1; i >= 0; --i) {
h_top[i] = inertia_top_left[i+1] * links[i].axis_top + inertia_top_right[i+1] * links[i].axis_bottom;
h_bottom[i] = inertia_bottom_left[i+1] * links[i].axis_top + inertia_top_left[i+1].transpose() * links[i].axis_bottom;
btScalar val = SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, h_top[i], h_bottom[i]);
D[i] = val;
Y[i] = links[i].joint_torque
- SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1])
- SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
const int parent = links[i].parent;
// Ip += pXi * (Ii - hi hi' / Di) * iXp
const btScalar one_over_di = 1.0f / D[i];
const btMatrix3x3 TL = inertia_top_left[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]);
const btMatrix3x3 TR = inertia_top_right[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]);
const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]);
btMatrix3x3 r_cross;
r_cross.setValue(
0, -links[i].cached_r_vector[2], links[i].cached_r_vector[1],
links[i].cached_r_vector[2], 0, -links[i].cached_r_vector[0],
-links[i].cached_r_vector[1], links[i].cached_r_vector[0], 0);
inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1];
inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() *
(r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1];
// Zp += pXi * (Zi + Ii*ci + hi*Yi/Di)
btVector3 in_top, in_bottom, out_top, out_bottom;
const btScalar Y_over_D = Y[i] * one_over_di;
in_top = zero_acc_top_angular[i+1]
+ inertia_top_left[i+1] * coriolis_top_angular[i]
+ inertia_top_right[i+1] * coriolis_bottom_linear[i]
+ Y_over_D * h_top[i];
in_bottom = zero_acc_bottom_linear[i+1]
+ inertia_bottom_left[i+1] * coriolis_top_angular[i]
+ inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]
+ Y_over_D * h_bottom[i];
InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
in_top, in_bottom, out_top, out_bottom);
zero_acc_top_angular[parent+1] += out_top;
zero_acc_bottom_linear[parent+1] += out_bottom;
}
// Second 'upward' loop
// (part of TreeForwardDynamics in Mirtich)
if (fixed_base)
{
accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
}
else
{
if (num_links > 0)
{
//Matrix<btScalar, 6, 6> Imatrix;
//Imatrix.block<3,3>(0,0) = inertia_top_left[0];
//Imatrix.block<3,3>(3,0) = inertia_bottom_left[0];
//Imatrix.block<3,3>(0,3) = inertia_top_right[0];
//Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose();
//cached_imatrix_lu.reset(new Eigen::LU<Matrix<btScalar, 6, 6> >(Imatrix)); // TODO: Avoid memory allocation here?
cached_inertia_top_left = inertia_top_left[0];
cached_inertia_top_right = inertia_top_right[0];
cached_inertia_lower_left = inertia_bottom_left[0];
cached_inertia_lower_right= inertia_top_left[0].transpose();
}
btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
float result[6];
solveImatrix(rhs_top, rhs_bot, result);
// printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
for (int i = 0; i < 3; ++i) {
accel_top[0][i] = -result[i];
accel_bottom[0][i] = -result[i+3];
}
}
// now do the loop over the links
for (int i = 0; i < num_links; ++i) {
const int parent = links[i].parent;
SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
accel_top[parent+1], accel_bottom[parent+1],
accel_top[i+1], accel_bottom[i+1]);
joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * links[i].axis_top;
accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * links[i].axis_bottom;
}
// transform base accelerations back to the world frame.
btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
output[0] = omegadot_out[0];
output[1] = omegadot_out[1];
output[2] = omegadot_out[2];
btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
output[3] = vdot_out[0];
output[4] = vdot_out[1];
output[5] = vdot_out[2];
// Final step: add the accelerations (times dt) to the velocities.
applyDeltaVee(output, dt);
}
void ConvertRotationToBull(const QAngle& angles, btMatrix3x3& bull) {
RadianEuler radian(angles);
Quaternion q(radian);
btQuaternion quat(q.x, q.z, -q.y, q.w);
bull.setRotation(quat);
}
btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Cooper(btScalar step) const
{
#if 0
dReal h = callContext->m_stepperCallContext->m_stepSize; // Step size
dVector3 L; // Compute angular momentum
dMultiply0_331(L, I, b->avel);
#endif
btVector3 inertiaLocal = getLocalInertia();
btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
btVector3 L = inertiaTensorWorld*getAngularVelocity();
btMatrix3x3 Itild(0, 0, 0, 0, 0, 0, 0, 0, 0);
#if 0
for (int ii = 0; ii<12; ++ii) {
Itild[ii] = Itild[ii] * h + I[ii];
}
#endif
btSetCrossMatrixMinus(Itild, L*step);
Itild += inertiaTensorWorld;
#if 0
// Compute a new effective 'inertia tensor'
// for the implicit step: the cross-product
// matrix of the angular momentum plus the
// old tensor scaled by the timestep.
// Itild may not be symmetric pos-definite,
// but we can still use it to compute implicit
// gyroscopic torques.
dMatrix3 Itild = { 0 };
dSetCrossMatrixMinus(Itild, L, 4);
for (int ii = 0; ii<12; ++ii) {
Itild[ii] = Itild[ii] * h + I[ii];
}
#endif
L *= step;
//Itild may not be symmetric pos-definite
btMatrix3x3 itInv = Itild.inverse();
Itild = inertiaTensorWorld * itInv;
btMatrix3x3 ident(1,0,0,0,1,0,0,0,1);
Itild -= ident;
#if 0
// Scale momentum by inverse time to get
// a sort of "torque"
dScaleVector3(L, dRecip(h));
// Invert the pseudo-tensor
dMatrix3 itInv;
// This is a closed-form inversion.
// It's probably not numerically stable
// when dealing with small masses with
// a large asymmetry.
// An LU decomposition might be better.
if (dInvertMatrix3(itInv, Itild) != 0) {
// "Divide" the original tensor
// by the pseudo-tensor (on the right)
dMultiply0_333(Itild, I, itInv);
// Subtract an identity matrix
Itild[0] -= 1; Itild[5] -= 1; Itild[10] -= 1;
// This new inertia matrix rotates the
// momentum to get a new set of torques
// that will work correctly when applied
// to the old inertia matrix as explicit
// torques with a semi-implicit update
// step.
dVector3 tau0;
dMultiply0_331(tau0, Itild, L);
// Add the gyro torques to the torque
// accumulator
for (int ii = 0; ii<3; ++ii) {
b->tacc[ii] += tau0[ii];
}
#endif
btVector3 tau0 = Itild * L;
// printf("tau0 = %f,%f,%f\n",tau0.x(),tau0.y(),tau0.z());
return tau0;
}
btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Ewert(btScalar step) const
{
// use full newton-euler equations. common practice to drop the wxIw term. want it for better tumbling behavior.
// calculate using implicit euler step so it's stable.
const btVector3 inertiaLocal = getLocalInertia();
const btVector3 w0 = getAngularVelocity();
btMatrix3x3 I;
I = m_worldTransform.getBasis().scaled(inertiaLocal) *
m_worldTransform.getBasis().transpose();
// use newtons method to find implicit solution for new angular velocity (w')
// f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0
// df/dw' = I + 1xIw'*step + w'xI*step
btVector3 w1 = w0;
// one step of newton's method
{
const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I);
const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I);
const btMatrix3x3 dfw_inv = dfw.inverse();
btVector3 dw;
dw = dfw_inv*fw;
w1 -= dw;
}
btVector3 gf = (w1 - w0);
return gf;
}
void btRigidBody::integrateVelocities(btScalar step)
{
if (isStaticOrKinematicObject())
return;
m_linearVelocity += m_totalForce * (m_inverseMass * step);
m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
#define MAX_ANGVEL SIMD_HALF_PI
/// clamp angular velocity. collision calculations will fail on higher angular velocities
btScalar angvel = m_angularVelocity.length();
if (angvel*step > MAX_ANGVEL)
{
m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
}
}
btQuaternion btRigidBody::getOrientation() const
{
btQuaternion orn;
m_worldTransform.getBasis().getRotation(orn);
return orn;
}
Geometry::Matrix3x3 toMatrix3x3(const btMatrix3x3& btm){
Geometry::Matrix3x3 tmp;
for(int i = 0; i<3; ++i)
tmp.setRow(i,toVec3(btm.getRow(i)));
return tmp;
}
