Change MuJoCo engine source code function-spacing convention from 3 blank lines to 2
PiperOrigin-RevId: 813754244 Change-Id: I6836e41c3b021cb727e922c25c60f629b9814c93
This commit is contained in:
Yuval Tassa
committed by
Copybara-Service
Copybara-Service
parent
c439628f82
commit
edbdb5195c
@@ -52,7 +52,6 @@ void mju_rotVecQuat(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// negate quaternion
|
||||
void mju_negQuat(mjtNum res[4], const mjtNum quat[4]) {
|
||||
res[0] = quat[0];
|
||||
@@ -62,7 +61,6 @@ void mju_negQuat(mjtNum res[4], const mjtNum quat[4]) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// multiply quaternions
|
||||
void mju_mulQuat(mjtNum res[4], const mjtNum qa[4], const mjtNum qb[4]) {
|
||||
mjtNum tmp[4] = {
|
||||
@@ -78,7 +76,6 @@ void mju_mulQuat(mjtNum res[4], const mjtNum qa[4], const mjtNum qb[4]) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// multiply quaternion and axis
|
||||
void mju_mulQuatAxis(mjtNum res[4], const mjtNum quat[4], const mjtNum axis[3]) {
|
||||
mjtNum tmp[4] = {
|
||||
@@ -94,7 +91,6 @@ void mju_mulQuatAxis(mjtNum res[4], const mjtNum quat[4], const mjtNum axis[3])
|
||||
}
|
||||
|
||||
|
||||
|
||||
// convert axisAngle to quaternion
|
||||
void mju_axisAngle2Quat(mjtNum res[4], const mjtNum axis[3], mjtNum angle) {
|
||||
// zero angle: null quat
|
||||
@@ -116,7 +112,6 @@ void mju_axisAngle2Quat(mjtNum res[4], const mjtNum axis[3], mjtNum angle) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// convert quaternion (corresponding to orientation difference) to 3D velocity
|
||||
void mju_quat2Vel(mjtNum res[3], const mjtNum quat[4], mjtNum dt) {
|
||||
mjtNum axis[3] = {quat[1], quat[2], quat[3]};
|
||||
@@ -133,7 +128,6 @@ void mju_quat2Vel(mjtNum res[3], const mjtNum quat[4], mjtNum dt) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Subtract quaternions, express as 3D velocity: qb*quat(res) = qa.
|
||||
void mju_subQuat(mjtNum res[3], const mjtNum qa[4], const mjtNum qb[4]) {
|
||||
// qdif = neg(qb)*qa
|
||||
@@ -146,7 +140,6 @@ void mju_subQuat(mjtNum res[3], const mjtNum qa[4], const mjtNum qb[4]) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// convert quaternion to 3D rotation matrix
|
||||
void mju_quat2Mat(mjtNum res[9], const mjtNum quat[4]) {
|
||||
// null quat: identity
|
||||
@@ -189,7 +182,6 @@ void mju_quat2Mat(mjtNum res[9], const mjtNum quat[4]) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// convert 3D rotation matrix to quaternion
|
||||
void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]) {
|
||||
// q0 largest
|
||||
@@ -228,7 +220,6 @@ void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// time-derivative of quaternion, given 3D rotational velocity
|
||||
void mju_derivQuat(mjtNum res[4], const mjtNum quat[4], const mjtNum vel[3]) {
|
||||
res[0] = 0.5*(-vel[0]*quat[1] - vel[1]*quat[2] - vel[2]*quat[3]);
|
||||
@@ -238,7 +229,6 @@ void mju_derivQuat(mjtNum res[4], const mjtNum quat[4], const mjtNum vel[3]) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// integrate quaternion given 3D angular velocity
|
||||
void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale) {
|
||||
mjtNum angle, tmp[4], qrot[4];
|
||||
@@ -252,7 +242,6 @@ void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// compute quaternion performing rotation from z-axis to given vector
|
||||
void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) {
|
||||
mjtNum axis[3], a, vn[3] = {vec[0], vec[1], vec[2]}, z[3] = {0, 0, 1};
|
||||
@@ -287,7 +276,6 @@ void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// extract 3D rotation from an arbitrary 3x3 matrix
|
||||
static const mjtNum rotEPS = 1e-9;
|
||||
int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]) {
|
||||
@@ -328,7 +316,6 @@ int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
//------------------------------ pose operations (quat, pos) ---------------------------------------
|
||||
|
||||
// multiply two poses
|
||||
@@ -345,7 +332,6 @@ void mju_mulPose(mjtNum posres[3], mjtNum quatres[4],
|
||||
}
|
||||
|
||||
|
||||
|
||||
// negate pose
|
||||
void mju_negPose(mjtNum posres[3], mjtNum quatres[4], const mjtNum pos[3], const mjtNum quat[4]) {
|
||||
// qres = neg(quat)
|
||||
@@ -357,7 +343,6 @@ void mju_negPose(mjtNum posres[3], mjtNum quatres[4], const mjtNum pos[3], const
|
||||
}
|
||||
|
||||
|
||||
|
||||
// transform vector by pose
|
||||
void mju_trnVecPose(mjtNum res[3], const mjtNum pos[3], const mjtNum quat[4], const mjtNum vec[3]) {
|
||||
// res = quat*vec + pos
|
||||
@@ -366,7 +351,6 @@ void mju_trnVecPose(mjtNum res[3], const mjtNum pos[3], const mjtNum quat[4], co
|
||||
}
|
||||
|
||||
|
||||
|
||||
//------------------------------ spatial algebra ---------------------------------------------------
|
||||
|
||||
// vector cross-product, 3D
|
||||
@@ -382,7 +366,6 @@ void mju_cross(mjtNum res[3], const mjtNum a[3], const mjtNum b[3]) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// cross-product for motion vector
|
||||
void mju_crossMotion(mjtNum res[6], const mjtNum vel[6], const mjtNum v[6]) {
|
||||
res[0] = -vel[2]*v[1] + vel[1]*v[2];
|
||||
@@ -398,7 +381,6 @@ void mju_crossMotion(mjtNum res[6], const mjtNum vel[6], const mjtNum v[6]) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// cross-product for force vectors
|
||||
void mju_crossForce(mjtNum res[6], const mjtNum vel[6], const mjtNum f[6]) {
|
||||
res[0] = -vel[2]*f[1] + vel[1]*f[2];
|
||||
@@ -414,7 +396,6 @@ void mju_crossForce(mjtNum res[6], const mjtNum vel[6], const mjtNum f[6]) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// express inertia in com-based frame
|
||||
void mju_inertCom(mjtNum res[10], const mjtNum inert[3], const mjtNum mat[9],
|
||||
const mjtNum dif[3], mjtNum mass) {
|
||||
@@ -449,7 +430,6 @@ void mju_inertCom(mjtNum res[10], const mjtNum inert[3], const mjtNum mat[9],
|
||||
}
|
||||
|
||||
|
||||
|
||||
// multiply 6D vector (rotation, translation) by 6D inertia matrix
|
||||
void mju_mulInertVec(mjtNum res[6], const mjtNum i[10], const mjtNum v[6]) {
|
||||
res[0] = i[0]*v[0] + i[3]*v[1] + i[4]*v[2] - i[8]*v[4] + i[7]*v[5];
|
||||
@@ -461,7 +441,6 @@ void mju_mulInertVec(mjtNum res[6], const mjtNum i[10], const mjtNum v[6]) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// express motion axis in com-based frame
|
||||
void mju_dofCom(mjtNum res[6], const mjtNum axis[3], const mjtNum offset[3]) {
|
||||
// hinge
|
||||
@@ -478,7 +457,6 @@ void mju_dofCom(mjtNum res[6], const mjtNum axis[3], const mjtNum offset[3]) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// multiply dof matrix (6-by-n, transposed) by vector (n-by-1)
|
||||
void mju_mulDofVec(mjtNum* res, const mjtNum* dof, const mjtNum* vec, int n) {
|
||||
if (n == 1) {
|
||||
@@ -491,7 +469,6 @@ void mju_mulDofVec(mjtNum* res, const mjtNum* dof, const mjtNum* vec, int n) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// transform 6D motion or force vector between frames
|
||||
// flg_force: determines vector type (motion or force)
|
||||
// rotnew2old: rotation that maps vectors from new to old frame,
|
||||
@@ -526,7 +503,6 @@ void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_force,
|
||||
}
|
||||
|
||||
|
||||
|
||||
// make 3D frame given X axis (normal) and possibly Y axis (tangent 1)
|
||||
void mju_makeFrame(mjtNum frame[9]) {
|
||||
mjtNum tmp[3];
|
||||
@@ -557,7 +533,6 @@ void mju_makeFrame(mjtNum frame[9]) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// convert sequence of Euler angles (radians) to quaternion
|
||||
// seq[0,1,2] must be in 'xyzXYZ', lower/upper-case mean intrinsic/extrinsic rotations
|
||||
void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq) {
|
||||
|
||||
Reference in New Issue
Block a user