//bilateral constraint between two dynamic objects
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
btRigidBody& body2, const btVector3& pos2,
btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
{
(void)timeStep;
(void)distance;
btScalar normalLenSqr = normal.length2();
btAssert(btFabs(normalLenSqr) < btScalar(1.1));
if (normalLenSqr > btScalar(1.1))
{
impulse = btScalar(0.);
return;
}
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
body2.getCenterOfMassTransform().getBasis().transpose(),
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
body2.getInvInertiaDiagLocal(),body2.getInvMass());
btScalar jacDiagAB = jac.getDiagonal();
btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
btScalar rel_vel = jac.getRelativeVelocity(
body1.getLinearVelocity(),
body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
body2.getLinearVelocity(),
body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
btScalar a;
a=jacDiagABInv;
rel_vel = normal.dot(vel);
//todo: move this into proper structure
btScalar contactDamping = btScalar(0.2);
#ifdef ONLY_USE_LINEAR_MASS
btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
impulse = - contactDamping * rel_vel * massTerm;
#else
btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
#endif
}
//bilateral constraint between two dynamic objects
void RaycastCar::resolveSingleBilateral(btRigidBody & body1,
const btVector3 & pos1,
btRigidBody & body2,
const btVector3 & pos2,
const btVector3 & normal,
btScalar & impulse)
{
btScalar normalLenSqr = normal.length2();
btAssert(btFabs(normalLenSqr) < btScalar(1.1f));
if (normalLenSqr > btScalar(1.1f))
{
impulse = btScalar(0.0f);
return;
}
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
body2.getCenterOfMassTransform().getBasis().transpose(),
rel_pos1,
rel_pos2,
normal,
body1.getInvInertiaDiagLocal(),
body1.getInvMass(),
body2.getInvInertiaDiagLocal(),
body2.getInvMass());
btScalar jacDiagAB = jac.getDiagonal();
btScalar jacDiagABInv = btScalar(1.0f) / jacDiagAB;
btScalar rel_vel = jac.getRelativeVelocity
(body1.getLinearVelocity(),
body1.getCenterOfMassTransform().getBasis().transpose()*body1.getAngularVelocity(),
body2.getLinearVelocity(),
body2.getCenterOfMassTransform().getBasis().transpose()*body2.getAngularVelocity());
btScalar velocityImpulse = -1.0f * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
}
///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;
}
virtual const btVector3
&getVelocity() const {return m_body->getLinearVelocity();}
