virtual ~RigidObject()
{
if(body->getMotionState())
delete body->getMotionState();
world->removeRigidBody(body);
delete body;
}
virtual ~RigidObject()
{
if(body)
{
if(body->getMotionState() != NULL)
delete body->getMotionState();
// if(body->getCollisionShape() != NULL)
// delete body->getCollisionShape();
world->removeRigidBody(body);
delete body;
}
}
bool inside(vec3f pos, float fudge)
{
mat3f rot;
//if(ctr == nullptr)
{
btTransform trans;
rigid_body->getMotionState()->getWorldTransform(trans);
btVector3 bpos = trans.getOrigin();
btQuaternion bq = trans.getRotation();
quaternion q = {{bq.x(), bq.y(), bq.z(), bq.w()}};
rot = q.get_rotation_matrix();
vec3f v = {bpos.x(), bpos.y(), bpos.z()};
vec3f rel = pos - v;
return within(b, rot.transp() * rel, fudge);
}
/*rot = ctr->rot_quat.get_rotation_matrix();
vec3f my_pos = xyz_to_vec(ctr->pos);
return within(b, rot.transp() * (pos - my_pos));*/
}
void updateBall()
{
if (_ptrBall)
// TODO use _motionState ?
_rigidBody.getMotionState()->getWorldTransform(
_ptrBall->_transform);
}
void glDraw() {
if (!model) return;
body1->getMotionState()->getWorldTransform(t);
t.getOpenGLMatrix(m);
glPushMatrix();
glMultMatrixf(m);
model->glDraw();
glPopMatrix();
}
quaternion get_quat()
{
btTransform newTrans;
rigid_body->getMotionState()->getWorldTransform(newTrans);
btQuaternion bq = newTrans.getRotation();
return {{bq.x(), bq.y(), bq.z(), bq.w()}};
}
vec3f get_pos()
{
btTransform newTrans;
rigid_body->getMotionState()->getWorldTransform(newTrans);
btVector3 bq = newTrans.getOrigin();
return {bq.x(), bq.y(), bq.z()};
}
void triMeshApp::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
if(m_convexRigidBody->getMotionState())
delete m_convexRigidBody->getMotionState();
m_dynamicsWorld->removeRigidBody(m_convexRigidBody);
delete m_convexRigidBody;
if(m_concaveRigidBody->getMotionState())
delete m_concaveRigidBody->getMotionState();
m_dynamicsWorld->removeRigidBody(m_concaveRigidBody);
delete m_concaveRigidBody;
if(m_hfRigidBody->getMotionState())
delete m_hfRigidBody->getMotionState();
m_dynamicsWorld->removeRigidBody(m_hfRigidBody);
delete m_hfRigidBody;
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
void set_trans(cl_float4 clpos, quaternion m)
{
mat3f mat_diff = base_diff.get_rotation_matrix();
mat3f current_hand = m.get_rotation_matrix();
mat3f my_rot = current_hand * mat_diff;
quaternion n;
n.load_from_matrix(my_rot);
vec3f absolute_pos = {clpos.x, clpos.y, clpos.z};
///current hand does not take into account the rotation offset when grabbing
///ie we'll double rotate
vec3f offset_rot = current_hand * offset;
vec3f pos = absolute_pos + offset_rot;
btTransform newTrans;
//rigid_body->getMotionState()->getWorldTransform(newTrans);
newTrans.setOrigin(btVector3(pos.v[0], pos.v[1], pos.v[2]));
newTrans.setRotation(btQuaternion(n.x(), n.y(), n.z(), n.w()));
rigid_body->getMotionState()->setWorldTransform(newTrans);
//rigid_body->setInterpolationWorldTransform(newTrans);
//if(ctr)
// ctr->set_pos(conv_implicit<cl_float4>(pos));
slide_parent_init = true;
slide_saved_parent = absolute_pos;
remote_pos = pos;
remote_rot = n;
kinematic_old = kinematic_current;
kinematic_current = xyzf_to_vec(rigid_body->getWorldTransform().getOrigin());
}
virtual void operator()(osg::Node* node, osg::NodeVisitor* nv)
{
btScalar m[16];
btDefaultMotionState* myMotionState = (btDefaultMotionState*) _body->getMotionState();
myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m);
osg::Matrixf mat(m);
// to move our ShapeNode according to gravity, we do something like this?:
osg::Matrixf diff = osg::Matrix::inverse(previousMatrix_) * mat;
spin::ShapeNode *shp = dynamic_cast<spin::ShapeNode*>(node);
/*
if (shp)
{
//osg::Vec3 t = diff.getTrans() - osg::Vec3(2,0,0);
//shp->translate(t.x(),t.y(),t.z());
shp->setTranslation(mat.getTrans().x(),mat.getTrans().y(),mat.getTrans().z());
}
else
{
osg::PositionAttitudeTransform *pat = dynamic_cast<osg::PositionAttitudeTransform*>(node);
if (pat)
{
pat->setPosition(mat.getTrans());
pat->setAttitude(mat.getRotate());
//std::cout << mat.getTrans().x() << ", " << mat.getTrans().y() << ", " << mat.getTrans().z() << std::endl;
}
}
*/
//_body->getMotionState()->setWorldTransform(trans);
_body->setWorldTransform(trans);
previousMatrix_ = mat;
traverse(node, nv);
}
///milliseconds
void tick(float ftime, CommonRigidBodyBase* bullet_scene)
{
kinematic_source s;
s.pos = &remote_pos;
s.rot = &remote_rot;
bullet_scene->setKinematicSource(rigid_body, s);
//should_hand_collide = false;
if(time_elapsed_since_release_ms >= time_needed_since_release_to_recollide_ms && should_hand_collide)
{
make_collide_hands(bullet_scene);
}
if(!should_hand_collide)
{
make_no_collide_hands(bullet_scene);
}
time_elapsed_since_release_ms += ftime;
//printf("%f timeelapsed\n", time_elapsed_since_release_ms);
if(frame_wait_restore > 0)
{
frame_wait_restore--;
if(frame_wait_restore == 0)
{
vec3f avg = {0,0,0};
int n = 0;
//if(history.size() > 0)
// history.pop_back();
/*if(history.size() > 0)
history.pop_back();*/
for(auto& i : history)
{
//if(n == history.size() / 2)
// break;
avg += i;
n++;
}
if(n > 0)
avg = avg / n;
//if(n > 0)
// rigid_body->setLinearVelocity({avg.v[0], avg.v[1], avg.v[2]});
//rigid_body->setLinearVelocity(bullet_scene->getBodyAvgVelocity(rigid_body));
}
}
///so the avg velocity is wrong, because it updates out of 'phase' with this function
if(is_kinematic && last_world_id != bullet_scene->info.internal_step_id)
{
//rigid_body->saveKinematicState(1/60.f);
btVector3 vel = rigid_body->getLinearVelocity();
btVector3 angular = rigid_body->getAngularVelocity();
//vec3f pos = get_pos();
//printf("pos %f %f %f\n", pos.v[0], pos.v[1], pos.v[2]);
avg_vel = avg_vel + (vec3f){vel.x(), vel.y(), vel.z()};
avg_vel = avg_vel / 2.f;
history.push_back({vel.x(), vel.y(), vel.z()});
if(history.size() > max_history)
history.pop_front();
//angular_stability_history
//printf("vel %f %f %f\n", vel.x(), vel.y(), vel.z());
}
angular_stability_history.push_back(get_scaled_angular_stability());
if(angular_stability_history.size() > max_stability_history)
angular_stability_history.pop_front();
/*if(!is_kinematic)
{
history.clear();
}*/
if(is_kinematic && slide_timer < slide_time_ms && slide_towards_parent)
{
offset = offset * 0.95f + ideal_offset * 0.05f;
}
if(self_owned)
{
btTransform trans;
rigid_body->getMotionState()->getWorldTransform(trans);
vec3f pos = xyzf_to_vec(trans.getOrigin());
quaternion qrot = convert_from_bullet_quaternion(trans.getRotation());
ctr->set_pos(conv_implicit<cl_float4>(pos));
ctr->set_rot_quat(qrot);
}
slide_timer += ftime;
last_ftime = ftime;
last_world_id = bullet_scene->info.internal_step_id;
}
