void btRigidBody::applyGravity()
{
if (isStaticOrKinematicObject())
return;
applyCentralForce(m_gravity);
}
void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
{
if (isStaticOrKinematicObject())
{
m_interpolationWorldTransform = m_worldTransform;
} else
{
m_interpolationWorldTransform = xform;
}
m_interpolationLinearVelocity = getLinearVelocity();
m_interpolationAngularVelocity = getAngularVelocity();
m_worldTransform = xform;
updateInertiaTensor();
}
void World::
reset()
{
btCollisionObjectArray a = dynamicsWorld->getCollisionObjectArray();
for (int i = 0; i < a.size(); i++) {
auto o = a[i];
if (!o->isStaticOrKinematicObject()) {
std::cout << "Object: " << o->getUserPointer() << std::endl;
btTransform t = o->getWorldTransform();
btVector3 v = t.getOrigin();
if (v.getY() < -10) {
std::shared_ptr<app::gl::AppObject> new_object;
dynamicsWorld->removeCollisionObject(o);
for (auto i = objects.begin(); i != objects.end(); ++i) {
if (i->get() == o->getUserPointer()) {
new_object = std::make_shared<app::gl::AppObject>(*(i->get()));
v.setX(disx(gen));
v.setY(disy(gen));
v.setZ(disz(gen));
t.setOrigin(v);
new_object->setWorldTransform(t);
objects.erase(i);
break;
}
}
addToWorld(new_object,
btRigidBody::btRigidBodyConstructionInfo(new_object->getMass(),
nullptr, nullptr, new_object->getInitialInertia()));
} else {
v.setX(disx(gen));
v.setY(disy(gen));
v.setZ(disz(gen));
t.setOrigin(v);
o->setWorldTransform(t);
o->setInterpolationLinearVelocity(btVector3(0,0,0));
o->setInterpolationAngularVelocity(btVector3(0,0,0));
o->setActivationState(1);
o->activate(true);
}
}
}
}
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;
}
}
void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
{
btAssert(!std::isnan(xform.getOrigin().getX()));
btAssert(!std::isnan(xform.getOrigin().getY()));
btAssert(!std::isnan(xform.getOrigin().getZ()));
btAssert(!std::isnan(xform.getRotation().getX()));
btAssert(!std::isnan(xform.getRotation().getY()));
btAssert(!std::isnan(xform.getRotation().getZ()));
btAssert(!std::isnan(xform.getRotation().getW()));
if (isStaticOrKinematicObject())
{
m_interpolationWorldTransform = m_worldTransform;
} else
{
m_interpolationWorldTransform = xform;
}
m_interpolationLinearVelocity = getLinearVelocity();
m_interpolationAngularVelocity = getAngularVelocity();
m_worldTransform = xform;
updateInertiaTensor();
}
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;
}
