//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();}