Add private function mju_fillInt
PiperOrigin-RevId: 845217293 Change-Id: I604372ab7ea7f4d47d9427d36dc8cd78e60d84ca
This commit is contained in:
committed by
Copybara-Service
parent
477a14dcb2
commit
2b9940bc67
@@ -678,16 +678,13 @@ void mju_addScl(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, mjtNum scl,
|
||||
|
||||
// normalize vector, return length before normalization
|
||||
mjtNum mju_normalize(mjtNum* res, int n) {
|
||||
mjtNum norm = (mjtNum)mju_sqrt(mju_dot(res, res, n));
|
||||
mjtNum normInv;
|
||||
mjtNum norm = mju_sqrt(mju_dot(res, res, n));
|
||||
|
||||
if (norm < mjMINVAL) {
|
||||
res[0] = 1;
|
||||
for (int i=1; i < n; i++) {
|
||||
res[i] = 0;
|
||||
}
|
||||
mju_zero(res + 1, n - 1);
|
||||
} else {
|
||||
normInv = 1/norm;
|
||||
mjtNum normInv = 1 / norm;
|
||||
for (int i=0; i < n; i++) {
|
||||
res[i] *= normInv;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user