//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 }
float get_angular_velocity() { btVector3 vel = rigid_body->getAngularVelocity(); vec3f ang = xyzf_to_vec(vel); return ang.length(); }
//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; }
void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) { int i; // linear btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA); btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB); btVector3 vel = velA - velB; for(i = 0; i < 3; i++) { const btVector3& normal = m_jacLin[i].m_linearJointAxis; btScalar rel_vel = normal.dot(vel); // calculate positional error btScalar depth = m_depth[i]; // get parameters btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); // calcutate and apply impulse btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i]; btVector3 impulse_vector = normal * normalImpulse; rbA.applyImpulse( impulse_vector, m_relPosA); rbB.applyImpulse(-impulse_vector, m_relPosB); if(m_poweredLinMotor && (!i)) { // apply linear motor if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce) { btScalar desiredMotorVel = m_targetLinMotorVelocity; btScalar motor_relvel = desiredMotorVel + rel_vel; normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; // clamp accumulated impulse btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse); if(new_acc > m_maxLinMotorForce) { new_acc = m_maxLinMotorForce; } btScalar del = new_acc - m_accumulatedLinMotorImpulse; if(normalImpulse < btScalar(0.0)) { normalImpulse = -del; } else { normalImpulse = del; } m_accumulatedLinMotorImpulse = new_acc; // apply clamped impulse impulse_vector = normal * normalImpulse; rbA.applyImpulse( impulse_vector, m_relPosA); rbB.applyImpulse(-impulse_vector, m_relPosB); } } } // angular // get axes in world space btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0); const btVector3& angVelA = rbA.getAngularVelocity(); const btVector3& angVelB = rbB.getAngularVelocity(); btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); btVector3 angAorthog = angVelA - angVelAroundAxisA; btVector3 angBorthog = angVelB - angVelAroundAxisB; btVector3 velrelOrthog = angAorthog-angBorthog; //solve orthogonal angular velocity correction btScalar len = velrelOrthog.length(); if (len > btScalar(0.00001)) { btVector3 normal = velrelOrthog.normalized(); btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal); velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; } //solve angular positional correction btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep); btScalar len2 = angularError.length(); if (len2>btScalar(0.00001)) { btVector3 normal2 = angularError.normalized(); btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2); angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; } // apply impulse rbA.applyTorqueImpulse(-velrelOrthog+angularError); rbB.applyTorqueImpulse(velrelOrthog-angularError); btScalar impulseMag; //solve angular limits if(m_solveAngLim) { impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep; impulseMag *= m_kAngle * m_softnessLimAng; } else { impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep; impulseMag *= m_kAngle * m_softnessDirAng; } btVector3 impulse = axisA * impulseMag; rbA.applyTorqueImpulse(impulse); rbB.applyTorqueImpulse(-impulse); //apply angular motor if(m_poweredAngMotor) { if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce) { btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB; btScalar projRelVel = velrel.dot(axisA); btScalar desiredMotorVel = m_targetAngMotorVelocity; btScalar motor_relvel = desiredMotorVel - projRelVel; btScalar angImpulse = m_kAngle * motor_relvel; // clamp accumulated impulse btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse); if(new_acc > m_maxAngMotorForce) { new_acc = m_maxAngMotorForce; } btScalar del = new_acc - m_accumulatedAngMotorImpulse; if(angImpulse < btScalar(0.0)) { angImpulse = -del; } else { angImpulse = del; } m_accumulatedAngMotorImpulse = new_acc; // apply clamped impulse btVector3 motorImp = angImpulse * axisA; m_rbA.applyTorqueImpulse(motorImp); m_rbB.applyTorqueImpulse(-motorImp); } } } // btSliderConstraint::solveConstraint()