| 150 | } |
| 151 | |
| 152 | void clamp_velocity_norm( |
| 153 | std::vector<float> & velocity, |
| 154 | const std::vector<float> & reference, |
| 155 | float threshold) { |
| 156 | if (threshold <= 0.0F) { |
| 157 | return; |
| 158 | } |
| 159 | require_same_size(velocity.size(), reference.size(), "velocity/reference size mismatch"); |
| 160 | float velocity_norm = 0.0F; |
| 161 | float reference_norm = 0.0F; |
| 162 | for (size_t i = 0; i < velocity.size(); ++i) { |
| 163 | velocity_norm += velocity[i] * velocity[i]; |
| 164 | reference_norm += reference[i] * reference[i]; |
| 165 | } |
| 166 | velocity_norm = std::sqrt(velocity_norm); |
| 167 | reference_norm = std::sqrt(reference_norm) + 1e-10F; |
| 168 | const float scale = std::min(1.0F, threshold * reference_norm / (velocity_norm + 1e-10F)); |
| 169 | for (float & value : velocity) { |
| 170 | value *= scale; |
| 171 | } |
| 172 | } |
| 173 | |
| 174 | std::vector<float> euler_step( |
| 175 | const std::vector<float> & x, |