From 0eec09dd606ff1ebc3e2f10e4ca02c695e9f0b55 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Tue, 16 Jun 2026 08:35:43 -0700 Subject: [PATCH] Add runtime assertions to mji functions. PiperOrigin-RevId: 933106771 Change-Id: I523cdceb89c90611f27983731c49fd4887034431 --- src/engine/engine_inline.h | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/engine/engine_inline.h b/src/engine/engine_inline.h index 9532271b..d5f97a97 100644 --- a/src/engine/engine_inline.h +++ b/src/engine/engine_inline.h @@ -15,6 +15,8 @@ #ifndef MUJOCO_SRC_ENGINE_ENGINE_INLINE_H_ #define MUJOCO_SRC_ENGINE_ENGINE_INLINE_H_ +#include + #include #include @@ -144,6 +146,7 @@ mjtNum mji__normalize3(mjtNum vec[3]) { // multiply vector by 3D rotation matrix static inline void mji_mulMatVec3(mjtNum* restrict res, const mjtNum mat[9], const mjtNum vec[3]) { + assert(res != vec); res[0] = mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2]; res[1] = mat[3]*vec[0] + mat[4]*vec[1] + mat[5]*vec[2]; res[2] = mat[6]*vec[0] + mat[7]*vec[1] + mat[8]*vec[2]; @@ -153,6 +156,7 @@ void mji_mulMatVec3(mjtNum* restrict res, const mjtNum mat[9], const mjtNum vec[ // multiply vector by transposed 3D rotation matrix static inline void mji_mulMatTVec3(mjtNum* restrict res, const mjtNum mat[9], const mjtNum vec[3]) { + assert(res != vec); res[0] = mat[0]*vec[0] + mat[3]*vec[1] + mat[6]*vec[2]; res[1] = mat[1]*vec[0] + mat[4]*vec[1] + mat[7]*vec[2]; res[2] = mat[2]*vec[0] + mat[5]*vec[1] + mat[8]*vec[2]; @@ -162,6 +166,7 @@ void mji_mulMatTVec3(mjtNum* restrict res, const mjtNum mat[9], const mjtNum vec // multiply 3x3 matrices, static inline void mji_mulMatMat3(mjtNum* restrict res, const mjtNum mat1[9], const mjtNum mat2[9]) { + assert(res != mat1 && res != mat2); res[0] = mat1[0]*mat2[0] + mat1[1]*mat2[3] + mat1[2]*mat2[6]; res[1] = mat1[0]*mat2[1] + mat1[1]*mat2[4] + mat1[2]*mat2[7]; res[2] = mat1[0]*mat2[2] + mat1[1]*mat2[5] + mat1[2]*mat2[8]; @@ -177,6 +182,7 @@ void mji_mulMatMat3(mjtNum* restrict res, const mjtNum mat1[9], const mjtNum mat // multiply 3x3 matrices, first argument transposed static inline void mji_mulMatTMat3(mjtNum* restrict res, const mjtNum mat1[9], const mjtNum mat2[9]) { + assert(res != mat1 && res != mat2); res[0] = mat1[0]*mat2[0] + mat1[3]*mat2[3] + mat1[6]*mat2[6]; res[1] = mat1[0]*mat2[1] + mat1[3]*mat2[4] + mat1[6]*mat2[7]; res[2] = mat1[0]*mat2[2] + mat1[3]*mat2[5] + mat1[6]*mat2[8]; @@ -192,6 +198,7 @@ void mji_mulMatTMat3(mjtNum* restrict res, const mjtNum mat1[9], const mjtNum ma // transpose 3x3 matrix static inline void mji_transpose3(mjtNum* restrict res, const mjtNum mat[9]) { + assert(res != mat); res[0] = mat[0]; res[1] = mat[3]; res[2] = mat[6]; @@ -244,6 +251,7 @@ mjtNum mji__normalize4(mjtNum vec[4]) { // rotate vector by quaternion static inline void mji_rotVecQuat(mjtNum* restrict res, const mjtNum vec[3], const mjtNum quat[4]) { + assert(res != quat); // null quat: copy vec if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) { mji_copy3(res, vec); @@ -277,6 +285,7 @@ void mji_negQuat(mjtNum* restrict res, const mjtNum quat[4]) { // multiply quaternions static inline void mji_mulQuat(mjtNum* restrict res, const mjtNum qa[4], const mjtNum qb[4]) { + assert(res != qa && res != qb); res[0] = qa[0]*qb[0] - qa[1]*qb[1] - qa[2]*qb[2] - qa[3]*qb[3]; res[1] = qa[0]*qb[1] + qa[1]*qb[0] + qa[2]*qb[3] - qa[3]*qb[2]; res[2] = qa[0]*qb[2] - qa[1]*qb[3] + qa[2]*qb[0] + qa[3]*qb[1]; @@ -287,6 +296,7 @@ void mji_mulQuat(mjtNum* restrict res, const mjtNum qa[4], const mjtNum qb[4]) { // multiply quaternion and axis static inline void mji_mulQuatAxis(mjtNum* restrict res, const mjtNum quat[4], const mjtNum axis[3]) { + assert(res != quat && res != axis); res[0] = -quat[1]*axis[0] - quat[2]*axis[1] - quat[3]*axis[2]; res[1] = quat[0]*axis[0] + quat[2]*axis[2] - quat[3]*axis[1]; res[2] = quat[0]*axis[1] + quat[3]*axis[0] - quat[1]*axis[2]; @@ -349,6 +359,7 @@ void mji_subQuat(mjtNum* restrict res, const mjtNum qa[4], const mjtNum qb[4]) { // convert 3D rotation matrix to quaternion static inline void mji_mat2Quat(mjtNum* restrict quat, const mjtNum mat[9]) { + assert(quat != mat); // q0 largest if (mat[0]+mat[4]+mat[8]>0) { quat[0] = 0.5 * mju_sqrt(1 + mat[0] + mat[4] + mat[8]); @@ -388,6 +399,7 @@ void mji_mat2Quat(mjtNum* restrict quat, const mjtNum mat[9]) { // integrate quaternion given 3D angular velocity static inline void mji_quatIntegrate(mjtNum* restrict quat, const mjtNum vel[3], mjtNum scale) { + assert(quat != vel); mjtNum angle, tmp[4], qrot[4]; // form local rotation quaternion, apply @@ -405,6 +417,7 @@ void mji_quatIntegrate(mjtNum* restrict quat, const mjtNum vel[3], mjtNum scale) // vector cross-product, 3D static inline void mji_cross(mjtNum* restrict res, const mjtNum a[3], const mjtNum b[3]) { + assert(res != a && res != b); res[0] = a[1]*b[2] - a[2]*b[1]; res[1] = a[2]*b[0] - a[0]*b[2]; res[2] = a[0]*b[1] - a[1]*b[0]; @@ -414,6 +427,7 @@ void mji_cross(mjtNum* restrict res, const mjtNum a[3], const mjtNum b[3]) { // cross-product for motion vector static inline void mji_crossMotion(mjtNum* restrict res, const mjtNum vel[6], const mjtNum v[6]) { + assert(res != vel && res != v); res[0] = -vel[2]*v[1] + vel[1]*v[2]; res[1] = vel[2]*v[0] - vel[0]*v[2]; res[2] = -vel[1]*v[0] + vel[0]*v[1]; @@ -430,6 +444,7 @@ void mji_crossMotion(mjtNum* restrict res, const mjtNum vel[6], const mjtNum v[6 // cross-product for force vectors static inline void mji_crossForce(mjtNum* restrict res, const mjtNum vel[6], const mjtNum f[6]) { + assert(res != vel && res != f); res[0] = -vel[2]*f[1] + vel[1]*f[2]; res[1] = vel[2]*f[0] - vel[0]*f[2]; res[2] = -vel[1]*f[0] + vel[0]*f[1];