void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVelocity, std::vector<btVector3>& velocities, std::vector<btScalar>& weights) { assert(index < (int)(_motors.size())); CharacterController::CharacterMotor& motor = _motors[index]; if (motor.hTimescale >= MAX_CHARACTER_MOTOR_TIMESCALE && motor.vTimescale >= MAX_CHARACTER_MOTOR_TIMESCALE) { // nothing to do return; } // rotate into motor-frame btVector3 axis = motor.rotation.getAxis(); btScalar angle = motor.rotation.getAngle(); btVector3 velocity = worldVelocity.rotate(axis, -angle); if (_state == State::Hover || motor.hTimescale == motor.vTimescale) { // modify velocity btScalar tau = dt / motor.hTimescale; if (tau > 1.0f) { tau = 1.0f; } velocity += (motor.velocity - velocity) * tau; // rotate back into world-frame velocity = velocity.rotate(axis, angle); // store the velocity and weight velocities.push_back(velocity); weights.push_back(tau); } else { // compute local UP btVector3 up = _currentUp.rotate(axis, -angle); // split velocity into horizontal and vertical components btVector3 vVelocity = velocity.dot(up) * up; btVector3 hVelocity = velocity - vVelocity; btVector3 vTargetVelocity = motor.velocity.dot(up) * up; btVector3 hTargetVelocity = motor.velocity - vTargetVelocity; // modify each component separately btScalar maxTau = 0.0f; if (motor.hTimescale < MAX_CHARACTER_MOTOR_TIMESCALE) { btScalar tau = dt / motor.hTimescale; if (tau > 1.0f) { tau = 1.0f; } maxTau = tau; hVelocity += (hTargetVelocity - hVelocity) * tau; } if (motor.vTimescale < MAX_CHARACTER_MOTOR_TIMESCALE) { btScalar tau = dt / motor.vTimescale; if (tau > 1.0f) { tau = 1.0f; } if (tau > maxTau) { maxTau = tau; } vVelocity += (vTargetVelocity - vVelocity) * tau; } // add components back together and rotate into world-frame velocity = (hVelocity + vVelocity).rotate(axis, angle); // store velocity and weights velocities.push_back(velocity); weights.push_back(maxTau); } }