chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add spaces around comparison operators in engine source files.

Browse Source 
PiperOrigin-RevId: 535989348
Change-Id: I883f7e82351299933c49b35a31842b5d8d6aea04
This commit is contained in:
Yuval Tassa
2023-05-28 05:01:55 -07:00
committed by Copybara-Service
parent d40c395917
commit 455b1cd2e2
29 changed files with 2224 additions and 2219 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_util_solve.c
+124 -124
View File
@@ -35,7 +35,7 @@ int mju_cholFactor(mjtNum* mat, int n, mjtNum mindiag) {
mjtNum tmp;
// in-place Cholesky factorization
for (int j=0; j<n; j++) {
for (int j=0; j < n; j++) {
// compute new diagonal
tmp = mat[j*(n+1)];
if (j) {
@@ -43,7 +43,7 @@ int mju_cholFactor(mjtNum* mat, int n, mjtNum mindiag) {
}
// correct diagonal values below threshold
if (tmp<mindiag) {
if (tmp < mindiag) {
tmp = mindiag;
rank--;
}
@@ -53,7 +53,7 @@ int mju_cholFactor(mjtNum* mat, int n, mjtNum mindiag) {
// process off-diagonal entries
tmp = 1/mat[j*(n+1)];
for (int i=j+1; i<n; i++) {
for (int i=j+1; i < n; i++) {
mat[i*n+j] = (mat[i*n+j] - mju_dot(mat+i*n, mat+j*n, j)) * tmp;
}
}
@@ -66,12 +66,12 @@ int mju_cholFactor(mjtNum* mat, int n, mjtNum mindiag) {
// Cholesky solve
void mju_cholSolve(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int n) {
// copy if source and destination are different
if (res!=vec) {
if (res != vec) {
mju_copy(res, vec, n);
}
// forward substitution: solve L*res = vec
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
if (i) {
res[i] -= mju_dot(mat+i*n, res, i);
}
@@ -81,9 +81,9 @@ void mju_cholSolve(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int n) {
}
// backward substitution: solve L'*res = res
for (int i=n-1; i>=0; i--) {
if (i<n-1) {
for (int j=i+1; j<n; j++) {
for (int i=n-1; i >= 0; i--) {
if (i < n-1) {
for (int j=i+1; j < n; j++) {
res[i] -= mat[j*n+i] * res[j];
}
}
@@ -99,12 +99,12 @@ int mju_cholUpdate(mjtNum* mat, mjtNum* x, int n, int flg_plus) {
int rank = n;
mjtNum r, c, cinv, s, Lkk, tmp;
for (int k=0; k<n; k++) {
for (int k=0; k < n; k++) {
if (x[k]) {
// prepare constants
Lkk = mat[k*(n+1)];
tmp = Lkk*Lkk + (flg_plus ? x[k]*x[k] : -x[k]*x[k]);
if (tmp<mjMINVAL) {
if (tmp < mjMINVAL) {
tmp = mjMINVAL;
rank--;
}
@@ -118,17 +118,17 @@ int mju_cholUpdate(mjtNum* mat, mjtNum* x, int n, int flg_plus) {
// update mat
if (flg_plus) {
for (int i=k+1; i<n; i++) {
for (int i=k+1; i < n; i++) {
mat[i*n+k] = (mat[i*n+k] + s*x[i])*cinv;
}
} else {
for (int i=k+1; i<n; i++) {
for (int i=k+1; i < n; i++) {
mat[i*n+k] = (mat[i*n+k] - s*x[i])*cinv;
}
}
// update x
for (int i=k+1; i<n; i++) {
for (int i=k+1; i < n; i++) {
x[i] = c*x[i] - s*mat[i*n+k];
}
}
@@ -153,26 +153,26 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag,
mjtNum* sparse_buf = mj_stackAlloc(d, n);
// shrink rows so that rownnz ends at diagonal
for (int r=0; r<n; r++) {
for (int r=0; r < n; r++) {
// shrink
while (rownnz[r]>0 && colind[rowadr[r]+rownnz[r]-1]>r) {
while (rownnz[r] > 0 && colind[rowadr[r]+rownnz[r]-1] > r) {
rownnz[r]--;
}
// check
if (rownnz[r]==0 || colind[rowadr[r]+rownnz[r]-1]!=r) {
if (rownnz[r] == 0 || colind[rowadr[r]+rownnz[r]-1] != r) {
mju_error("Matrix must have non-zero diagonal in mju_cholFactorSparse");
}
}
// backpass over rows
for (int r=n-1; r>=0; r--) {
for (int r=n-1; r >= 0; r--) {
// get rownnz and rowadr for row r
int nnz = rownnz[r], adr = rowadr[r];
// update row r diagonal
mjtNum tmp = mat[adr+nnz-1];
if (tmp<mindiag) {
if (tmp < mindiag) {
tmp = mindiag;
rank--;
}
@@ -180,12 +180,12 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag,
tmp = 1/mat[adr+nnz-1];
// update row r before diagonal
for (int i=0; i<nnz-1; i++) {
for (int i=0; i < nnz-1; i++) {
mat[adr+i] *= tmp;
}
// update row c<r where mat(r,c)!=0
for (int i=0; i<nnz-1; i++) {
for (int i=0; i < nnz-1; i++) {
// get column index
int c = colind[adr+i];
@@ -212,7 +212,7 @@ void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int
mju_copy(res, vec, n);
// vec <- L^-T vec
for (int i=n-1; i>=0; i--) {
for (int i=n-1; i >= 0; i--) {
if (res[i]) {
// get rowadr[i], rownnz[i]
const int adr = rowadr[i], nnz = rownnz[i];
@@ -222,19 +222,19 @@ void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int
mjtNum tmp = res[i];
// x(j) -= L(i,j)*x(i), j=0:i-1
for (int j=0; j<nnz-1; j++) {
for (int j=0; j < nnz-1; j++) {
res[colind[adr+j]] -= mat[adr+j]*tmp;
}
}
}
// vec <- L^-1 vec
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
// get rowadr[i], rownnz[i]
const int adr = rowadr[i], nnz = rownnz[i];
// x(i) -= sum_j L(i,j)*x(j), j=0:i-1
if (nnz>1) {
if (nnz > 1) {
res[i] -= mju_dotSparse(mat+adr, res, nnz-1, colind+adr);
// modulo AVX, the above line does
// for (int j=0; j<nnz-1; j++)
@@ -260,13 +260,13 @@ int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus,
// backpass over rows corresponding to non-zero x(r)
int rank = n, i = x_nnz - 1;
while (i>=0) {
while (i >= 0) {
// get rownnz and rowadr for this row
int nnz = rownnz[x_ind[i]], adr = rowadr[x_ind[i]];
// compute quantities
mjtNum tmp = mat[adr+nnz-1]*mat[adr+nnz-1] + (flg_plus ? x[i]*x[i] : -x[i]*x[i]);
if (tmp<mjMINVAL) {
if (tmp < mjMINVAL) {
tmp = mjMINVAL;
rank--;
}
@@ -283,7 +283,7 @@ int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus,
sparse_buf, buf_ind);
// check for size change
if (new_nnz!=nnz-1) {
if (new_nnz != nnz-1) {
mju_error("Varying sparsity pattern in mju_cholUpdateSparse");
}
@@ -314,7 +314,7 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense,
mjtNum mindiag = -1;
// sparse part, including sparse-sparse and sparse-dense
for (int j=0; j<nsparse; j++) {
for (int j=0; j < nsparse; j++) {
// number of non-zeros left of (j,j)
int width_jj = mjMIN(j, nband-1);
@@ -325,16 +325,16 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense,
int adr_jj = (j+1)*nband-1;
// compute L(j,j), before sqrt
mjtNum left_ij = width_jj>0 ? mju_dot(mat+adr_jj-width_jj, mat+adr_jj-width_jj, width_jj) : 0;
mjtNum left_ij = width_jj > 0 ? mju_dot(mat+adr_jj-width_jj, mat+adr_jj-width_jj, width_jj) : 0;
mjtNum Ljj = diagadd + diagmul*mat[adr_jj] + mat[adr_jj] - left_ij;
// update mindiag
if (Ljj<mindiag || mindiag<0) {
if (Ljj < mindiag || mindiag < 0) {
mindiag = Ljj;
}
// stop if rank-deficient
if (Ljj<mjMINVAL) {
if (Ljj < mjMINVAL) {
return 0;
}
@@ -343,7 +343,7 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense,
mjtNum scale = 1/Ljj;
// compute L(i,j) for i>j, sparse part
for (int i=j+1; i<=j+height; i++) {
for (int i=j+1; i <= j+height; i++) {
// number of non-zeros left of (i,j)
int width_ij = mjMIN(j, nband-1-i+j);
@@ -351,18 +351,18 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense,
int adr_ij = (i+1)*nband-1-i+j;
// in-place computation of L(i,j)
left_ij = width_ij>0 ? mju_dot(mat+adr_jj-width_ij, mat+adr_ij-width_ij, width_ij) : 0;
left_ij = width_ij > 0 ? mju_dot(mat+adr_jj-width_ij, mat+adr_ij-width_ij, width_ij) : 0;
mat[adr_ij] = scale * (mat[adr_ij] - left_ij);
}
// compute L(i,j) for i>j, dense part
for (int i=nsparse; i<ntotal; i++) {
for (int i=nsparse; i < ntotal; i++) {
// address of (i,j)
int adr_ij = nsparse*nband + (i-nsparse)*ntotal + j;
// in-place computation of L(i,j)
// number of non-zeros left of (i,j) now equals width_jj
left_ij = width_jj>0 ? mju_dot(mat+adr_jj-width_jj, mat+adr_ij-width_jj, width_jj) : 0;
left_ij = width_jj > 0 ? mju_dot(mat+adr_jj-width_jj, mat+adr_ij-width_jj, width_jj) : 0;
mat[adr_ij] = scale * (mat[adr_ij] - left_ij);
}
@@ -371,7 +371,7 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense,
}
// dense part
for (int j=nsparse; j<ntotal; j++) {
for (int j=nsparse; j < ntotal; j++) {
// address of (j,j)
int adr_jj = nsparse*nband + (j-nsparse)*ntotal + j;
@@ -380,12 +380,12 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense,
mju_dot(mat+adr_jj-j, mat+adr_jj-j, j);
// update mindiag
if (Ljj<mindiag || mindiag<0) {
if (Ljj < mindiag || mindiag < 0) {
mindiag = Ljj;
}
// stop if rank-deficient
if (Ljj<mjMINVAL) {
if (Ljj < mjMINVAL) {
return 0;
}
@@ -394,7 +394,7 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense,
mjtNum scale = 1/Ljj;
// compute L(i,j) for i>j
for (int i=j+1; i<ntotal; i++) {
for (int i=j+1; i < ntotal; i++) {
// address of off-diagonal element
int adr_ij = adr_jj + ntotal*(i-j);
@@ -417,14 +417,14 @@ void mju_cholSolveBand(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
int width, height, nsparse = ntotal - ndense;
// copy into result if different
if (res!=vec) {
if (res != vec) {
mju_copy(res, vec, ntotal);
}
//------- forward substitution: solve L*res = vec
// sparse part
for (int i=0; i<nsparse; i++) {
for (int i=0; i < nsparse; i++) {
// number of non-zeros left of (i,i)
width = mjMIN(i, nband-1);
@@ -437,7 +437,7 @@ void mju_cholSolveBand(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
}
// dense part
for (int i=nsparse; i<ntotal; i++) {
for (int i=nsparse; i < ntotal; i++) {
res[i] -= mju_dot(mat+nsparse*nband+(i-nsparse)*ntotal, res, i);
// diagonal
@@ -447,8 +447,8 @@ void mju_cholSolveBand(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
//------- backward substitution: solve L'*res = res
// dense part
for (int i=ntotal-1; i>=nsparse; i--) {
for (int j=i+1; j<ntotal; j++) {
for (int i=ntotal-1; i >= nsparse; i--) {
for (int j=i+1; j < ntotal; j++) {
res[i] -= mat[nsparse*nband+(j-nsparse)*ntotal+i] * res[j];
}
@@ -457,16 +457,16 @@ void mju_cholSolveBand(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
}
// sparse part
for (int i=nsparse-1; i>=0; i--) {
for (int i=nsparse-1; i >= 0; i--) {
// number of non-zeros below (i,i), sparse part
height = mjMIN(nsparse-1-i, nband-1);
// sparse rows
for (int j=i+1; j<=i+height; j++)
for (int j=i+1; j <= i+height; j++)
res[i] -= mat[(j+1)*nband-1-(j-i)] * res[j];
// dense rows
for (int j=nsparse; j<ntotal; j++)
for (int j=nsparse; j < ntotal; j++)
res[i] -= mat[nsparse*nband+(j-nsparse)*ntotal+i] * res[j];
// diagonal
@@ -481,7 +481,7 @@ int mju_bandDiag(int i, int ntotal, int nband, int ndense) {
int nsparse = ntotal-ndense;
// sparse part
if (i<nsparse) {
if (i < nsparse) {
return i*nband + nband-1;
}
@@ -502,7 +502,7 @@ void mju_band2Dense(mjtNum* res, const mjtNum* mat, int ntotal, int nband, int n
mju_zero(res, ntotal*ntotal);
// sparse part
for(int i=0; i<nsparse; i++) {
for(int i=0; i < nsparse; i++) {
// number of non-zeros left of (i,i)
int width = mjMIN(i, nband-1);
@@ -511,14 +511,14 @@ void mju_band2Dense(mjtNum* res, const mjtNum* mat, int ntotal, int nband, int n
}
// dense part
for(int i=nsparse; i<ntotal; i++) {
for(int i=nsparse; i < ntotal; i++) {
mju_copy(res + i*ntotal, mat + nsparse*nband + (i-nsparse)*ntotal, i+1);
}
// make symmetric
if (flg_sym) {
for(int i=0; i<ntotal; i++) {
for (int j=i+1; j<ntotal; j++) {
for(int i=0; i < ntotal; i++) {
for (int j=i+1; j < ntotal; j++) {
res[i*ntotal + j] = res[j*ntotal + i];
}
}
@@ -532,7 +532,7 @@ void mju_dense2Band(mjtNum* res, const mjtNum* mat, int ntotal, int nband, int n
int nsparse = ntotal-ndense;
// sparse part
for(int i=0; i<nsparse; i++) {
for(int i=0; i < nsparse; i++) {
// number of non-zeros left of (i,i)
int width = mjMIN(i, nband-1);
@@ -541,7 +541,7 @@ void mju_dense2Band(mjtNum* res, const mjtNum* mat, int ntotal, int nband, int n
}
// dense part
for(int i=nsparse; i<ntotal; i++) {
for(int i=nsparse; i < ntotal; i++) {
mju_copy(res + nsparse*nband + (i-nsparse)*ntotal, mat + i*ntotal, i+1);
}
}
@@ -554,13 +554,13 @@ void mju_bandMulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
int nsparse = ntotal-ndense;
// handle multiple vectors
for(int j=0; j<nvec; j++ ) {
for(int j=0; j < nvec; j++ ) {
// precompute pointer to corresponding vector in vec and res
const mjtNum* vec_j = vec + ntotal*j;
mjtNum* res_j = res + ntotal*j;
// sparse part
for(int i=0; i<nsparse; i++) {
for(int i=0; i < nsparse; i++) {
int width = mjMIN(i+1, nband);
int adr = i*nband + nband - width;
int offset = mjMAX(0, i-nband+1);
@@ -572,7 +572,7 @@ void mju_bandMulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
}
// dense part
for(int i=nsparse; i<ntotal; i++) {
for(int i=nsparse; i < ntotal; i++) {
int adr = nsparse*nband + (i-nsparse)*ntotal;
res_j[i] = mju_dot(mat+adr, vec_j, i+1);
if (flg_sym) {
@@ -596,28 +596,28 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
memcpy(remaining, rownnz, n*sizeof(int));
// diagonal elements (i,i)
for (int i=n-1; i>=0; i--) {
for (int i=n-1; i >= 0; i--) {
// get address of last remaining element of row i, adjust remaining counter
int ii = rowadr[i] + remaining[i] - 1;
remaining[i]--;
// make sure ii is on diagonal
if (colind[ii]!=i) {
if (colind[ii] != i) {
mju_error("missing diagonal element in mju_factorLUSparse");
}
// make sure diagonal is not too small
if (mju_abs(LU[ii])<mjMINVAL) {
if (mju_abs(LU[ii]) < mjMINVAL) {
mju_error("diagonal element too small in mju_factorLUSparse");
}
// rows j above i
for (int j=i-1; j>=0; j--) {
for (int j=i-1; j >= 0; j--) {
// get address of last remaining element of row j
int ji = rowadr[j] + remaining[j] - 1;
// process row j if (j,i) is non-zero
if (colind[ji]==i) {
if (colind[ji] == i) {
// adjust remaining counter
remaining[j]--;
@@ -627,15 +627,15 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
// (j,k) = (j,k) - (i,k) * (j,i) for k<i; handle incompatible sparsity
int icnt = rowadr[i], jcnt = rowadr[j];
while (jcnt<rowadr[j]+remaining[j]) {
while (jcnt < rowadr[j]+remaining[j]) {
// both non-zero
if (colind[icnt]==colind[jcnt]) {
if (colind[icnt] == colind[jcnt]) {
// update LU, advance counters
LU[jcnt++] -= LU[icnt++] * LUji;
}
// only (j,k) non-zero
else if (colind[icnt]>colind[jcnt]) {
else if (colind[icnt] > colind[jcnt]) {
// advance j counter
jcnt++;
}
@@ -647,7 +647,7 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
}
// make sure both rows fully processed
if (icnt!=rowadr[i]+remaining[i] || jcnt!=rowadr[j]+remaining[j]) {
if (icnt != rowadr[i]+remaining[i] || jcnt != rowadr[j]+remaining[j]) {
mju_error("row processing incomplete in mju_factorLUSparse");
}
}
@@ -655,8 +655,8 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
}
// make sure remaining points to diagonal
for (int i=0; i<n; i++) {
if (remaining[i]<0 || colind[rowadr[i]+remaining[i]]!=i) {
for (int i=0; i < n; i++) {
if (remaining[i] < 0 || colind[rowadr[i]+remaining[i]] != i) {
mju_error("unexpected sparse matrix structure in mju_factorLUSparse");
}
}
@@ -668,28 +668,28 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n,
const int* rownnz, const int* rowadr, const int* colind) {
//------------------ solve (U+I)*res = vec
for (int i=n-1; i>=0; i--) {
for (int i=n-1; i >= 0; i--) {
// init: diagonal of (U+I) is 1
res[i] = vec[i];
// res[i] -= sum_k>i res[k]*LU(i,k)
int j = rownnz[i] - 1;
while (colind[rowadr[i]+j]>i) {
while (colind[rowadr[i]+j] > i) {
res[i] -= res[colind[rowadr[i]+j]] * LU[rowadr[i]+j];
j--;
}
// make sure j points to diagonal
if (colind[rowadr[i]+j]!=i) {
if (colind[rowadr[i]+j] != i) {
mju_error("diagonal of U not reached in mju_factorLUSparse");
}
}
//------------------ solve L*res(new) = res
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
// res[i] -= sum_k<i res[k]*LU(i,k)
int j = 0;
while (colind[rowadr[i]+j]<i) {
while (colind[rowadr[i]+j] < i) {
res[i] -= res[colind[rowadr[i]+j]] * LU[rowadr[i]+j];
j++;
}
@@ -698,7 +698,7 @@ void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n,
res[i] /= LU[rowadr[i]+j];
// make sure j points to diagonal
if (colind[rowadr[i]+j]!=i) {
if (colind[rowadr[i]+j] != i) {
mju_error("diagonal of L not reached in mju_factorLUSparse");
}
}
@@ -720,7 +720,7 @@ int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9]
quat[1] = quat[2] = quat[3] = 0;
// Jacobi iteration
for (iter=0; iter<500; iter++) {
for (iter=0; iter < 500; iter++) {
// make quaternion matrix eigvec, compute D = eigvec'*mat*eigvec
mju_quat2Mat(eigvec, quat);
mju_mulMatTMat(tmp, eigvec, mat, 3, 3, 3);
@@ -732,11 +732,11 @@ int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9]
eigval[2] = D[8];
// find max off-diagonal element, set indices
if (fabs(D[1])>fabs(D[2]) && fabs(D[1])>fabs(D[5])) {
if (fabs(D[1]) > fabs(D[2]) && fabs(D[1]) > fabs(D[5])) {
rk = 0; // row
ck = 1; // column
rotk = 2; // rotation axis
} else if (fabs(D[2])>fabs(D[5])) {
} else if (fabs(D[2]) > fabs(D[5])) {
rk = 0;
ck = 2;
rotk = 1;
@@ -747,13 +747,13 @@ int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9]
}
// terminate if max off-diagonal element too small
if (fabs(D[3*rk+ck])<eigEPS) {
if (fabs(D[3*rk+ck]) < eigEPS) {
break;
}
// 2x2 symmetric Schur decomposition
tau = (D[4*ck]-D[4*rk])/(2*D[3*rk+ck]);
if (tau>=0) {
if (tau >= 0) {
t = 1.0/(tau + mju_sqrt(1 + tau*tau));
} else {
t = -1.0/(-tau + mju_sqrt(1 + tau*tau));
@@ -761,14 +761,14 @@ int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9]
c = 1.0/mju_sqrt(1 + t*t);
// terminate if cosine too close to 1
if (c>1.0-eigEPS) {
if (c > 1.0-eigEPS) {
break;
}
// express rotation as quaternion
tmp[1] = tmp[2] = tmp[3] = 0;
tmp[rotk+1] = (tau>=0 ? -mju_sqrt(0.5-0.5*c) : mju_sqrt(0.5-0.5*c));
if (rotk==1) {
tmp[rotk+1] = (tau >= 0 ? -mju_sqrt(0.5-0.5*c) : mju_sqrt(0.5-0.5*c));
if (rotk == 1) {
tmp[rotk+1] = -tmp[rotk+1];
}
tmp[0] = mju_sqrt(1.0 - tmp[rotk+1]*tmp[rotk+1]);
@@ -780,7 +780,7 @@ int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9]
}
// sort eigenvalues in decreasing order (bubblesort: 0, 1, 0)
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
int j1 = j%2; // lead index
if (eigval[j1] < eigval[j1+1]) {
@@ -825,12 +825,12 @@ int mju_QCQP2(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
// Newton iteration
la = 0;
for (int iter=0; iter<20; iter++) {
for (int iter=0; iter < 20; iter++) {
// det(A+la)
det = (A11+la)*(A22+la) - A12*A12;
// check SPD, with 1e-10 threshold
if (det<1e-10) {
if (det < 1e-10) {
res[0] = 0;
res[1] = 0;
return 0;
@@ -850,7 +850,7 @@ int mju_QCQP2(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
val = v1*v1 + v2*v2 - r*r;
// check for convergence, or initial solution inside constraint set
if (val<1e-10) {
if (val < 1e-10) {
break;
}
@@ -859,7 +859,7 @@ int mju_QCQP2(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
// compute update, exit if too small
mjtNum delta = -val/deriv;
if (delta<1e-10) {
if (delta < 1e-10) {
break;
}
@@ -871,7 +871,7 @@ int mju_QCQP2(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
res[0] = v1*d[0];
res[1] = v2*d[1];
return (la!=0);
return (la != 0);
}
@@ -897,7 +897,7 @@ int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
// Newton iteration
la = 0;
for (int iter=0; iter<20; iter++) {
for (int iter=0; iter < 20; iter++) {
// unscaled P
P11 = (A22+la)*(A33+la) - A23*A23;
P22 = (A11+la)*(A33+la) - A13*A13;
@@ -910,7 +910,7 @@ int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
det = (A11+la)*P11 + A12*P12 + A13*P13;
// check SPD, with 1e-10 threshold
if (det<1e-10) {
if (det < 1e-10) {
res[0] = 0;
res[1] = 0;
res[2] = 0;
@@ -937,7 +937,7 @@ int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
val = v1*v1 + v2*v2 + v3*v3 - r*r;
// check for convergence, or initial solution inside constraint set
if (val<1e-10) {
if (val < 1e-10) {
break;
}
@@ -947,7 +947,7 @@ int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
// compute update, exit if too small
mjtNum delta = -val/deriv;
if (delta<1e-10) {
if (delta < 1e-10) {
break;
}
@@ -960,7 +960,7 @@ int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
res[1] = v2*d[1];
res[2] = v3*d[2];
return (la!=0);
return (la != 0);
}
@@ -974,25 +974,25 @@ int mju_QCQP(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
mjtNum la, val, deriv, tmp[5];
// check size
if (n>5) {
if (n > 5) {
mju_error("mju_QCQP supports n up to 5");
}
// scale A,b so that constraint becomes x'*x <= r*r
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
b[i] = bin[i] * d[i];
for (int j=0; j<n; j++) {
for (int j=0; j < n; j++) {
A[j+i*n] = Ain[j+i*n] * d[i] * d[j];
}
}
// Newton iteration
la = 0;
for (int iter=0; iter<20; iter++) {
for (int iter=0; iter < 20; iter++) {
// make A+la
mju_copy(Ala, A, n*n);
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
Ala[i*(n+1)] += la;
}
@@ -1010,7 +1010,7 @@ int mju_QCQP(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
val = mju_dot(res, res, n) - r*r;
// check for convergence, or initial solution inside constraint set
if (val<1e-10) {
if (val < 1e-10) {
break;
}
@@ -1020,7 +1020,7 @@ int mju_QCQP(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
// compute update, exit if too small
mjtNum delta = -val/deriv;
if (delta<1e-10) {
if (delta < 1e-10) {
break;
}
@@ -1029,11 +1029,11 @@ int mju_QCQP(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
}
// undo scaling
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
res[i] = res[i] * d[i];
}
return (la!=0);
return (la != 0);
}
@@ -1119,7 +1119,7 @@ enum mjtStatusBoxQP {
// assumes symmetry of mat, ignores upper triangle
static mjtNum mulVecMatVecSym(const mjtNum* vec, const mjtNum* mat, int n) {
mjtNum res = 0;
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
res += vec[i] * mat[n*i+i] * vec[i]; // diagonal
res += 2 * vec[i] * mju_dot(mat+n*i, vec, i); // off-diagonal
}
@@ -1150,11 +1150,11 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
mjtNum sdotg, improvement=0, value=0, norm2=0;
// basic checks
if (n<=0) {
if (n <= 0) {
mju_error("mju_boxQP: problem size n must be positive");
}
if (upper && lower) {
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
if (lower[i] >= upper[i]) {
mju_error("mju_boxQP: upper bounds must be stricly larger than lower bounds");
}
@@ -1200,14 +1200,14 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
}
// full index set (no clamping)
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
index[i] = i;
}
}
// have bounds: clamp res
else {
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
if (lower) {
res[i] = mju_max(res[i], lower[i]);
}
@@ -1220,7 +1220,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
// ------ main loop
int iter, logptr = 0;
mjtNum oldvalue;
for (iter=0; iter<maxiter; iter++) {
for (iter=0; iter < maxiter; iter++) {
if (status != mjBOXQP_NO_DESCENT) {
break;
}
@@ -1236,14 +1236,14 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
mju_addTo(grad, g, n);
// find clamped dimensions
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
clamped[i] = ( lower && res[i] == lower[i] && grad[i] > 0 ) ||
( upper && res[i] == upper[i] && grad[i] < 0 );
}
// build index of free dimensions, count them
nfree = 0;
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
if (!clamped[i]) {
index[nfree++] = i;
}
@@ -1258,7 +1258,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
// re-factorize if clamped dimensions have changed
if (iter) {
factorize = 0;
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
if (clamped[i] != oldclamped[i]) {
factorize = 1;
break;
@@ -1267,26 +1267,26 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
}
// save last clamped
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
oldclamped[i] = clamped[i];
}
// get search direction: search = g + H_all,clamped * res_clamped
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
temp[i] = clamped[i] ? res[i] : 0;
}
mju_mulMatVec(search, H, temp, n, n);
mju_addTo(search, g, n);
// search = compress_free(search)
for (int i=0; i<nfree; i++) {
for (int i=0; i < nfree; i++) {
search[i] = search[index[i]];
}
// R = compress_free(H)
if (factorize) {
for (int i=0; i<nfree; i++) {
for (int j=0; j<i+1; j++) {
for (int i=0; i < nfree; i++) {
for (int j=0; j < i+1; j++) {
R[i*nfree+j] = H[index[i]*n+index[j]];
}
}
@@ -1307,7 +1307,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
// search_free = expand_free(-temp) - x_free
mju_zero(search, n);
for (int i=0; i<nfree; i++) {
for (int i=0; i < nfree; i++) {
search[index[i]] = -temp[i] -res[index[i]];
}
@@ -1315,13 +1315,13 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
// squared norm of free gradient
norm2 = 0;
for (int i=0; i<nfree; i++) {
for (int i=0; i < nfree; i++) {
mjtNum grad_i = grad[index[i]];
norm2 += grad_i*grad_i;
}
// small gradient: minimum found
if (norm2<mingrad) {
if (norm2 < mingrad) {
status = nfree == n ? mjBOXQP_UNBOUNDED : mjBOXQP_TOL_GRAD;
break;
}
@@ -1338,10 +1338,10 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
// candidate = clamp(x + step*search)
mju_scl(candidate, search, step, n);
mju_addTo(candidate, res, n);
for (int i=0; i<n; i++) {
if (lower && candidate[i]<lower[i]) {
for (int i=0; i < n; i++) {
if (lower && candidate[i] < lower[i]) {
candidate[i] = lower[i];
} else if (upper && candidate[i]>upper[i]) {
} else if (upper && candidate[i] > upper[i]) {
candidate[i] = upper[i];
}
}
@@ -1352,7 +1352,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
// increment and break if step is too small
nstep++;
step = step*backtrack;
if (step<minstep) {
if (step < minstep) {
status = mjBOXQP_MAX_LS_ITER;
break;
}
@@ -1376,7 +1376,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
}
// max iterations exceeded
if (iter==maxiter) {
if (iter == maxiter) {
status = mjBOXQP_MAX_ITER;
}