Alessio Quaglino
91c92279d2
Previously shell mode required cellcount=1 along at least one axis. This CL adds support for cellcount > 1 in all three axes by pinning interior grid nodes to the parent body and reconstructing their positions from boundary nodes via Transfinite Interpolation (TFI). PiperOrigin-RevId: 924314800 Change-Id: I8c2438f4866dd4133feed65f535a1ab69f0c9188
2378 lines
66 KiB
C
2378 lines
66 KiB
C
// Copyright 2021 DeepMind Technologies Limited
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
|
|
#include "engine/engine_util_misc.h"
|
|
|
|
#include <ctype.h>
|
|
#include <math.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
#include <mujoco/mjdata.h>
|
|
#include <mujoco/mjmacro.h>
|
|
#include <mujoco/mjmodel.h>
|
|
#include "engine/engine_array_safety.h"
|
|
#include "engine/engine_macro.h"
|
|
#include "engine/engine_util_blas.h"
|
|
#include "engine/engine_util_errmem.h"
|
|
#include "engine/engine_util_spatial.h"
|
|
|
|
//------------------------------ tendon wrapping ---------------------------------------------------
|
|
|
|
// check for intersection of two 2D line segments
|
|
static mjtBool is_intersect(const mjtNum* p1, const mjtNum* p2,
|
|
const mjtNum* p3, const mjtNum* p4) {
|
|
mjtNum a, b;
|
|
|
|
// compute determinant, check
|
|
mjtNum det = (p4[1]-p3[1])*(p2[0]-p1[0]) - (p4[0]-p3[0])*(p2[1]-p1[1]);
|
|
if (mju_abs(det) < mjMINVAL) {
|
|
return false;
|
|
}
|
|
|
|
// compute intersection point on each line
|
|
a = ((p4[0]-p3[0])*(p1[1]-p3[1]) - (p4[1]-p3[1])*(p1[0]-p3[0])) / det;
|
|
b = ((p2[0]-p1[0])*(p1[1]-p3[1]) - (p2[1]-p1[1])*(p1[0]-p3[0])) / det;
|
|
|
|
return ((a >= 0 && a <= 1 && b >= 0 && b <= 1) ? true : false);
|
|
}
|
|
|
|
|
|
// curve length along circle
|
|
static mjtNum length_circle(const mjtNum* p0, const mjtNum* p1, int ind, mjtNum radius) {
|
|
mjtNum p0n[2] = {p0[0], p0[1]};
|
|
mjtNum p1n[2] = {p1[0], p1[1]};
|
|
|
|
// compute angle between 0 and pi
|
|
mju_normalize(p0n, 2);
|
|
mju_normalize(p1n, 2);
|
|
mjtNum angle = mju_acos(mju_dot(p0n, p1n, 2));
|
|
|
|
// flip if necessary
|
|
mjtNum cross = p0[1]*p1[0]-p0[0]*p1[1];
|
|
if ((cross > 0 && ind) || (cross < 0 && !ind)) {
|
|
angle = 2*mjPI - angle;
|
|
}
|
|
|
|
return radius*angle;
|
|
}
|
|
|
|
|
|
// 2D circle wrap
|
|
// input: pair of 2D endpoints in end[4], optional 2D side point in side[2], radius
|
|
// output: return length of circular wrap or -1
|
|
// pair of 2D points in pnt[4]
|
|
static mjtNum wrap_circle(mjtNum pnt[4], const mjtNum end[4], const mjtNum* side, mjtNum radius) {
|
|
mjtNum sqlen0 = end[0]*end[0] + end[1]*end[1];
|
|
mjtNum sqlen1 = end[2]*end[2] + end[3]*end[3];
|
|
mjtNum sqrad = radius*radius;
|
|
|
|
// either point inside circle or circle too small: no wrap
|
|
if (sqlen0 < sqrad || sqlen1 < sqrad || radius < mjMINVAL) {
|
|
return -1;
|
|
}
|
|
|
|
// points too close: no wrap
|
|
mjtNum dif[2] = {end[2]-end[0], end[3]-end[1]};
|
|
mjtNum dd = dif[0]*dif[0] + dif[1]*dif[1];
|
|
if (dd < mjMINVAL) {
|
|
return -1;
|
|
}
|
|
|
|
// find nearest point on line segment to origin: a*dif + d0
|
|
mjtNum a = -(dif[0]*end[0]+dif[1]*end[1])/dd;
|
|
if (a < 0) {
|
|
a = 0;
|
|
} else if (a > 1) {
|
|
a = 1;
|
|
}
|
|
|
|
// check for intersection and side
|
|
mjtNum tmp[2] = {a*dif[0] + end[0], a*dif[1] + end[1]};
|
|
if (tmp[0]*tmp[0]+tmp[1]*tmp[1] > sqrad && (!side || mju_dot(side, tmp, 2) >= 0)) {
|
|
return -1;
|
|
}
|
|
|
|
mjtNum sqrt0 = mju_sqrt(sqlen0 - sqrad);
|
|
mjtNum sqrt1 = mju_sqrt(sqlen1 - sqrad);
|
|
|
|
// construct the two solutions, compute goodness
|
|
mjtNum sol[2][2][2], good[2];
|
|
for (int i=0; i < 2; i++) {
|
|
int sgn = (i == 0 ? 1 : -1);
|
|
|
|
sol[i][0][0] = (end[0]*sqrad + sgn*radius*end[1]*sqrt0)/sqlen0;
|
|
sol[i][0][1] = (end[1]*sqrad - sgn*radius*end[0]*sqrt0)/sqlen0;
|
|
sol[i][1][0] = (end[2]*sqrad - sgn*radius*end[3]*sqrt1)/sqlen1;
|
|
sol[i][1][1] = (end[3]*sqrad + sgn*radius*end[2]*sqrt1)/sqlen1;
|
|
|
|
// goodness: close to sd, or shorter path
|
|
if (side) {
|
|
mju_add(tmp, sol[i][0], sol[i][1], 2);
|
|
mju_normalize(tmp, 2);
|
|
good[i] = mju_dot(tmp, side, 2);
|
|
} else {
|
|
mju_sub(tmp, sol[i][0], sol[i][1], 2);
|
|
good[i] = -mju_dot(tmp, tmp, 2);
|
|
}
|
|
|
|
// penalize for intersection
|
|
if (is_intersect(end, sol[i][0], end+2, sol[i][1])) {
|
|
good[i] = -10000;
|
|
}
|
|
}
|
|
|
|
// select the better solution
|
|
int i = (good[0] > good[1] ? 0 : 1);
|
|
pnt[0] = sol[i][0][0];
|
|
pnt[1] = sol[i][0][1];
|
|
pnt[2] = sol[i][1][0];
|
|
pnt[3] = sol[i][1][1];
|
|
|
|
// check for intersection
|
|
if (is_intersect(end, pnt, end+2, pnt+2)) {
|
|
return -1;
|
|
}
|
|
|
|
// return curve length
|
|
return length_circle(sol[i][0], sol[i][1], i, radius);
|
|
}
|
|
|
|
|
|
// 2D inside wrap
|
|
// input: pair of 2D endpoints in end[4], radius
|
|
// output: pair of 2D points in pnt[4]; return 0 if wrap, -1 if no wrap
|
|
static mjtNum wrap_inside(mjtNum pnt[4], const mjtNum end[4], mjtNum radius) {
|
|
// algorithm parameters
|
|
const int maxiter = 20;
|
|
const mjtNum zinit = 1 - 1e-7;
|
|
const mjtNum tolerance = 1e-6;
|
|
|
|
// constants
|
|
mjtNum len0 = mju_norm(end, 2);
|
|
mjtNum len1 = mju_norm(end+2, 2);
|
|
mjtNum dif[2] = {end[2]-end[0], end[3]-end[1]};
|
|
mjtNum dd = dif[0]*dif[0] + dif[1]*dif[1];
|
|
|
|
// either point inside circle or circle too small: no wrap
|
|
if (len0 <= radius || len1 <= radius || radius < mjMINVAL || len0 < mjMINVAL || len1 < mjMINVAL) {
|
|
return -1;
|
|
}
|
|
|
|
// segment-circle intersection: no wrap
|
|
if (dd > mjMINVAL) {
|
|
// find nearest point on line segment to origin: d0 + a*dif
|
|
mjtNum a = -(dif[0]*end[0] + dif[1]*end[1]) / dd;
|
|
|
|
// in segment
|
|
if (a > 0 && a < 1) {
|
|
mjtNum tmp[2];
|
|
mju_addScl(tmp, end, dif, a, 2);
|
|
if (mju_norm(tmp, 2) <= radius) {
|
|
return -1;
|
|
}
|
|
}
|
|
}
|
|
|
|
// prepare default in case of numerical failure: average
|
|
pnt[0] = 0.5*(end[0] + end[2]);
|
|
pnt[1] = 0.5*(end[1] + end[3]);
|
|
mju_normalize(pnt, 2);
|
|
mju_scl(pnt, pnt, radius, 2);
|
|
pnt[2] = pnt[0];
|
|
pnt[3] = pnt[1];
|
|
|
|
// compute function parameters: asin(A*z) + asin(B*z) - 2*asin(z) + G = 0
|
|
mjtNum A = radius/len0;
|
|
mjtNum B = radius/len1;
|
|
mjtNum cosG = (len0*len0 + len1*len1 - dd) / (2*len0*len1);
|
|
if (cosG < -1+mjMINVAL) {
|
|
return -1;
|
|
} else if (cosG > 1-mjMINVAL) {
|
|
return 0;
|
|
}
|
|
mjtNum G = mju_acos(cosG);
|
|
|
|
// init
|
|
mjtNum z = zinit;
|
|
mjtNum f = mju_asin(A*z) + mju_asin(B*z) - 2*mju_asin(z) + G;
|
|
|
|
// make sure init is not on the other side
|
|
if (f > 0) {
|
|
return 0;
|
|
}
|
|
|
|
// Newton method
|
|
int iter;
|
|
for (iter=0; iter < maxiter && mju_abs(f) > tolerance; iter++) {
|
|
// derivative
|
|
mjtNum df = A/mju_max(mjMINVAL, mju_sqrt(1-z*z*A*A)) +
|
|
B/mju_max(mjMINVAL, mju_sqrt(1-z*z*B*B)) -
|
|
2/mju_max(mjMINVAL, mju_sqrt(1-z*z));
|
|
|
|
// check sign; SHOULD NOT OCCUR
|
|
if (df > -mjMINVAL) {
|
|
return 0;
|
|
}
|
|
|
|
// new point
|
|
mjtNum z1 = z - f/df;
|
|
|
|
// make sure we are moving to the left; SHOULD NOT OCCUR
|
|
if (z1 > z) {
|
|
return 0;
|
|
}
|
|
|
|
// update solution
|
|
z = z1;
|
|
f = mju_asin(A*z) + mju_asin(B*z) - 2*mju_asin(z) + G;
|
|
|
|
// exit if positive; SHOULD NOT OCCUR
|
|
if (f > tolerance) {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
// check convergence
|
|
if (iter >= maxiter) {
|
|
return 0;
|
|
}
|
|
|
|
// finalize: rotation by ang from vec = a or b, depending on cross(a,b) sign
|
|
mjtNum vec[2];
|
|
mjtNum ang;
|
|
if (end[0]*end[3] - end[1]*end[2] > 0) {
|
|
mju_copy(vec, end, 2);
|
|
ang = mju_asin(z) - mju_asin(A*z);
|
|
} else {
|
|
mju_copy(vec, end+2, 2);
|
|
ang = mju_asin(z) - mju_asin(B*z);
|
|
}
|
|
mju_normalize(vec, 2);
|
|
pnt[0] = radius*(mju_cos(ang)*vec[0] - mju_sin(ang)*vec[1]);
|
|
pnt[1] = radius*(mju_sin(ang)*vec[0] + mju_cos(ang)*vec[1]);
|
|
pnt[2] = pnt[0];
|
|
pnt[3] = pnt[1];
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
// wrap tendons around spheres and cylinders
|
|
// input: x0, x1: pair of 3D endpoints
|
|
// xpos, xmat, radius: position, orientation and radius of geom
|
|
// type: wrap type (mjtWrap)
|
|
// side: 3D position of sidesite
|
|
// output: return wrap length, -1 if no wrap
|
|
// wpnt: pair of 3D wrap points
|
|
mjtNum mju_wrap(mjtNum wpnt[6], const mjtNum x0[3], const mjtNum x1[3],
|
|
const mjtNum xpos[3], const mjtNum xmat[9], mjtNum radius,
|
|
int type, const mjtNum side[3]) {
|
|
// check object type; SHOULD NOT OCCUR
|
|
if (type != mjWRAP_SPHERE && type != mjWRAP_CYLINDER) {
|
|
mjERROR("unknown wrapping object type %d", type);
|
|
}
|
|
|
|
// map sites to wrap object's local frame
|
|
mjtNum tmp[3];
|
|
mju_sub3(tmp, x0, xpos);
|
|
mjtNum p[2][3];
|
|
mju_mulMatTVec3(p[0], xmat, tmp);
|
|
mju_sub3(tmp, x1, xpos);
|
|
mju_mulMatTVec3(p[1], xmat, tmp);
|
|
|
|
// too close to origin: return
|
|
if (mju_norm3(p[0]) < mjMINVAL || mju_norm3(p[1]) < mjMINVAL) {
|
|
return -1;
|
|
}
|
|
|
|
// construct 2D frame for circle wrap
|
|
mjtNum axis[2][3];
|
|
if (type == mjWRAP_SPHERE) {
|
|
// 1st axis = p0
|
|
mju_copy3(axis[0], p[0]);
|
|
mju_normalize3(axis[0]);
|
|
|
|
// normal to p0-0-p1 plane = cross(p0, p1)
|
|
mjtNum normal[3];
|
|
mju_cross(normal, p[0], p[1]);
|
|
mjtNum nrm = mju_normalize3(normal);
|
|
|
|
// if (p0, p1) parallel: different normal
|
|
if (nrm < mjMINVAL) {
|
|
// find max component of axis0
|
|
int i = 0;
|
|
if (mju_abs(axis[0][1]) > mju_abs(axis[0][0]) &&
|
|
mju_abs(axis[0][1]) > mju_abs(axis[0][2])) {
|
|
i = 1;
|
|
}
|
|
if (mju_abs(axis[0][2]) > mju_abs(axis[0][0]) &&
|
|
mju_abs(axis[0][2]) > mju_abs(axis[0][1])) {
|
|
i = 2;
|
|
}
|
|
|
|
// init second axis: 0 at i; 1 elsewhere
|
|
axis[1][0] = 1;
|
|
axis[1][1] = 1;
|
|
axis[1][2] = 1;
|
|
axis[1][i] = 0;
|
|
|
|
// recompute normal
|
|
mju_cross(normal, axis[0], axis[1]);
|
|
mju_normalize3(normal);
|
|
}
|
|
|
|
// 2nd axis = cross(normal, p0)
|
|
mju_cross(axis[1], normal, axis[0]);
|
|
mju_normalize3(axis[1]);
|
|
} else {
|
|
// 1st axis = x
|
|
axis[0][0] = 1;
|
|
axis[0][1] = axis[0][2] = 0;
|
|
|
|
// 2nd axis = y
|
|
axis[1][1] = 1;
|
|
axis[1][0] = axis[1][2] = 0;
|
|
}
|
|
|
|
// project points in 2D frame: p => d
|
|
mjtNum s[3], d[4], sd[2];
|
|
d[0] = mju_dot3(p[0], axis[0]);
|
|
d[1] = mju_dot3(p[0], axis[1]);
|
|
d[2] = mju_dot3(p[1], axis[0]);
|
|
d[3] = mju_dot3(p[1], axis[1]);
|
|
|
|
// handle sidesite
|
|
if (side) {
|
|
// side point: apply same projection as x0, x1
|
|
mju_sub3(tmp, side, xpos);
|
|
mju_mulMatTVec3(s, xmat, tmp);
|
|
|
|
// side point: project and rescale
|
|
sd[0] = mju_dot3(s, axis[0]);
|
|
sd[1] = mju_dot3(s, axis[1]);
|
|
mju_normalize(sd, 2);
|
|
mju_scl(sd, sd, radius, 2);
|
|
}
|
|
|
|
// apply inside wrap
|
|
mjtNum wlen;
|
|
mjtNum pnt[4];
|
|
if (side && mju_norm3(s) < radius) {
|
|
wlen = wrap_inside(pnt, d, radius);
|
|
}
|
|
|
|
// apply circle wrap
|
|
else {
|
|
wlen = wrap_circle(pnt, d, (side ? sd : NULL), radius);
|
|
}
|
|
|
|
// no wrap: return
|
|
if (wlen < 0) {
|
|
return -1;
|
|
}
|
|
|
|
// reconstruct 3D points in local frame: res
|
|
mjtNum res[6];
|
|
for (int i=0; i < 2; i++) {
|
|
// res = axis0*d0 + axis1*d1
|
|
mju_scl3(res+3*i, axis[0], pnt[2*i]);
|
|
mju_scl3(tmp, axis[1], pnt[2*i+1]);
|
|
mju_addTo3(res+3*i, tmp);
|
|
}
|
|
|
|
// cylinder: correct along z
|
|
if (type == mjWRAP_CYLINDER) {
|
|
// set vertical coordinates
|
|
mjtNum L0 = mju_sqrt((p[0][0]-res[0])*(p[0][0]-res[0]) + (p[0][1]-res[1])*(p[0][1]-res[1]));
|
|
mjtNum L1 = mju_sqrt((p[1][0]-res[3])*(p[1][0]-res[3]) + (p[1][1]-res[4])*(p[1][1]-res[4]));
|
|
res[2] = p[0][2] + (p[1][2] - p[0][2])*L0 / (L0+wlen+L1);
|
|
res[5] = p[0][2] + (p[1][2] - p[0][2])*(L0+wlen) / (L0+wlen+L1);
|
|
|
|
// correct wlen for height
|
|
mjtNum height = mju_abs(res[5] - res[2]);
|
|
wlen = mju_sqrt(wlen*wlen + height*height);
|
|
}
|
|
|
|
// map back to global frame: wpnt
|
|
mju_mulMatVec3(wpnt, xmat, res);
|
|
mju_mulMatVec3(wpnt+3, xmat, res+3);
|
|
mju_addTo3(wpnt, xpos);
|
|
mju_addTo3(wpnt+3, xpos);
|
|
|
|
return wlen;
|
|
}
|
|
|
|
|
|
//------------------------------ misc geometry -----------------------------------------------------
|
|
|
|
// all 3 semi-axes of a geom
|
|
void mju_geomSemiAxes(mjtNum semiaxes[3], const mjtNum size[3], mjtGeom type) {
|
|
switch (type) {
|
|
case mjGEOM_SPHERE:
|
|
semiaxes[0] = size[0];
|
|
semiaxes[1] = size[0];
|
|
semiaxes[2] = size[0];
|
|
break;
|
|
|
|
case mjGEOM_CAPSULE:
|
|
semiaxes[0] = size[0];
|
|
semiaxes[1] = size[0];
|
|
semiaxes[2] = size[1] + size[0];
|
|
break;
|
|
|
|
case mjGEOM_CYLINDER:
|
|
semiaxes[0] = size[0];
|
|
semiaxes[1] = size[0];
|
|
semiaxes[2] = size[1];
|
|
break;
|
|
|
|
default:
|
|
semiaxes[0] = size[0];
|
|
semiaxes[1] = size[1];
|
|
semiaxes[2] = size[2];
|
|
}
|
|
}
|
|
|
|
|
|
// return 1 if point is inside a primitive geom, 0 otherwise
|
|
int mju_insideGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3], mjtGeom type,
|
|
const mjtNum point[3]) {
|
|
// vector from geom to point
|
|
mjtNum vec[3];
|
|
mju_sub3(vec, point, pos);
|
|
|
|
// quick return for spheres, frame rotation not required
|
|
if (type == mjGEOM_SPHERE) {
|
|
return mju_dot3(vec, vec) < size[0]*size[0];
|
|
}
|
|
|
|
// rotate into local frame
|
|
mjtNum plocal[3];
|
|
mju_mulMatTVec3(plocal, mat, vec);
|
|
|
|
// handle other geom types
|
|
switch (type) {
|
|
case mjGEOM_CAPSULE: {
|
|
mjtNum z = plocal[2];
|
|
mjtNum z_clamped = mju_clip(z, -size[1], size[1]);
|
|
mjtNum z_dist_sq = (z - z_clamped) * (z - z_clamped);
|
|
return (plocal[0]*plocal[0] + plocal[1]*plocal[1] + z_dist_sq < size[0]*size[0]);
|
|
}
|
|
|
|
case mjGEOM_ELLIPSOID:
|
|
return (plocal[0]*plocal[0]/(size[0]*size[0]) +
|
|
plocal[1]*plocal[1]/(size[1]*size[1]) +
|
|
plocal[2]*plocal[2]/(size[2]*size[2]) < 1);
|
|
|
|
case mjGEOM_CYLINDER:
|
|
return (mju_abs(plocal[2]) < size[1] &&
|
|
plocal[0]*plocal[0] + plocal[1]*plocal[1] < size[0]*size[0]);
|
|
|
|
case mjGEOM_BOX:
|
|
return (mju_abs(plocal[0]) < size[0] &&
|
|
mju_abs(plocal[1]) < size[1] &&
|
|
mju_abs(plocal[2]) < size[2]);
|
|
|
|
case mjGEOM_PLANE:
|
|
return plocal[2] < 0;
|
|
|
|
default:
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
|
|
// compute ray origin and direction for pixel (col, row) in camera image
|
|
// for perspective: origin is unchanged, direction is computed
|
|
// for orthographic: direction is -Z in camera frame, origin is offset from camera center
|
|
void mju_camPixelRay(mjtNum origin[3], mjtNum direction[3],
|
|
const mjtNum cam_xpos[3], const mjtNum cam_xmat[9],
|
|
int col, int row, mjtNum fx, mjtNum fy, mjtNum cx, mjtNum cy,
|
|
int projection, mjtNum ortho_extent) {
|
|
// pixel center (row 0 = top of image)
|
|
mjtNum px = col + 0.5 - cx;
|
|
mjtNum py = row + 0.5 - cy;
|
|
|
|
if (projection == mjPROJ_PERSPECTIVE) {
|
|
// origin is camera position
|
|
mju_copy3(origin, cam_xpos);
|
|
|
|
// direction in camera frame: (x/fx, -y/fy, -1), then normalized
|
|
mjtNum dir_cam[3] = {px / fx, -py / fy, -1.0};
|
|
mju_mulMatVec3(direction, cam_xmat, dir_cam);
|
|
mju_normalize3(direction);
|
|
} else {
|
|
// orthographic: parallel rays, direction is -Z in camera frame
|
|
direction[0] = -cam_xmat[2];
|
|
direction[1] = -cam_xmat[5];
|
|
direction[2] = -cam_xmat[8];
|
|
|
|
// origin offset in camera frame (ortho_extent is full height, use half for each side)
|
|
mjtNum half_extent = ortho_extent / 2;
|
|
mjtNum offset_cam[3] = {px / fx * half_extent, -py / fy * half_extent, 0};
|
|
mjtNum offset_world[3];
|
|
mju_mulMatVec3(offset_world, cam_xmat, offset_cam);
|
|
mju_add3(origin, cam_xpos, offset_world);
|
|
}
|
|
}
|
|
|
|
|
|
// ----------------------------- flex interpolation ------------------------------------------------
|
|
|
|
// use shared shape functions from engine_util_misc.h
|
|
#define phi mju_flexPhi
|
|
#define dphi mju_flexDphi
|
|
|
|
// evaluate the deformation gradient at p using the nodal dof values
|
|
void mju_defGradient(mjtNum res[9], const mjtNum p[3], const mjtNum* dof, int order) {
|
|
int idx = 0;
|
|
mjtNum gradient[3];
|
|
mju_zero(res, 9);
|
|
for (int i = 0; i <= order; i++) {
|
|
for (int j = 0; j <= order; j++) {
|
|
for (int k = 0; k <= order; k++) {
|
|
gradient[0] = dphi(p[0], i, order) * phi(p[1], j, order) * phi(p[2], k, order);
|
|
gradient[1] = phi(p[0], i, order) * dphi(p[1], j, order) * phi(p[2], k, order);
|
|
gradient[2] = phi(p[0], i, order) * phi(p[1], j, order) * dphi(p[2], k, order);
|
|
res[0] += dof[3*idx+0] * gradient[0];
|
|
res[1] += dof[3*idx+0] * gradient[1];
|
|
res[2] += dof[3*idx+0] * gradient[2];
|
|
res[3] += dof[3*idx+1] * gradient[0];
|
|
res[4] += dof[3*idx+1] * gradient[1];
|
|
res[5] += dof[3*idx+1] * gradient[2];
|
|
res[6] += dof[3*idx+2] * gradient[0];
|
|
res[7] += dof[3*idx+2] * gradient[1];
|
|
res[8] += dof[3*idx+2] * gradient[2];
|
|
idx++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// evaluate the basis function at x for the i-th node
|
|
mjtNum mju_evalBasis(const mjtNum x[3], int i, int order) {
|
|
if (order == 1) {
|
|
return phi(x[2], i&1, order) * phi(x[1], i&2, order) * phi(x[0], i&4, order);
|
|
} else if (order == 2) {
|
|
return phi(x[2], i % 3, order) * phi(x[1], (i / 3) % 3, order) * phi(x[0], i / 9, order);
|
|
} else {
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
// evaluate the basis functions at x for all nodes in the cell
|
|
void mju_evalBasisArray(mjtNum* basis, const mjtNum x[3], int order) {
|
|
if (order == 1) {
|
|
mjtNum p[3][2] = {
|
|
{1 - x[0], x[0]},
|
|
{1 - x[1], x[1]},
|
|
{1 - x[2], x[2]}
|
|
};
|
|
int j = 0;
|
|
for (int i0=0; i0<2; i0++) {
|
|
mjtNum w0 = p[0][i0];
|
|
for (int i1=0; i1<2; i1++) {
|
|
mjtNum w01 = w0 * p[1][i1];
|
|
for (int i2=0; i2<2; i2++) {
|
|
basis[j++] = w01 * p[2][i2];
|
|
}
|
|
}
|
|
}
|
|
} else if (order == 2) {
|
|
mjtNum p[3][3];
|
|
for (int d=0; d<3; d++) {
|
|
for (int i=0; i<3; i++) {
|
|
p[d][i] = phi(x[d], i, 2);
|
|
}
|
|
}
|
|
int j = 0;
|
|
for (int i0=0; i0<3; i0++) {
|
|
mjtNum w0 = p[0][i0];
|
|
for (int i1=0; i1<3; i1++) {
|
|
mjtNum w01 = w0 * p[1][i1];
|
|
for (int i2=0; i2<3; i2++) {
|
|
basis[j++] = w01 * p[2][i2];
|
|
}
|
|
}
|
|
}
|
|
} else {
|
|
int npoint = (order + 1) * (order + 1) * (order + 1);
|
|
for (int j=0; j < npoint; j++) {
|
|
basis[j] = mju_evalBasis(x, j, order);
|
|
}
|
|
}
|
|
}
|
|
|
|
// map global parametric coord to cell-local coord and build node indices
|
|
// coord: [0,1]^3 parametric coordinates
|
|
// cellnum: cell counts (cx, cy, cz)
|
|
// order: interpolation order (1=trilinear, 2=triquadratic)
|
|
// local: output local parametric coordinates within cell [0,1]^3
|
|
// nodeindices: output array of global node indices for the cell (size (order+1)^3, may be NULL)
|
|
// returns: number of nodes per cell (order+1)^3
|
|
int mju_cellLookup(const mjtNum coord[3], const int cellnum[3], int order, mjtNum local[3],
|
|
int* nodeindices) {
|
|
int cx = cellnum[0], cy = cellnum[1], cz = cellnum[2];
|
|
|
|
// find containing cell
|
|
int ci = (int)mju_floor(coord[0] * cx);
|
|
int cj = (int)mju_floor(coord[1] * cy);
|
|
int ck = (int)mju_floor(coord[2] * cz);
|
|
ci = mjMIN(ci, cx - 1); ci = mjMAX(ci, 0);
|
|
cj = mjMIN(cj, cy - 1); cj = mjMAX(cj, 0);
|
|
ck = mjMIN(ck, cz - 1); ck = mjMAX(ck, 0);
|
|
|
|
// local parametric coordinates within cell
|
|
local[0] = mju_clip(coord[0] * cx - ci, 0, 1);
|
|
local[1] = mju_clip(coord[1] * cy - cj, 0, 1);
|
|
local[2] = mju_clip(coord[2] * cz - ck, 0, 1);
|
|
|
|
// build node indices for this cell
|
|
if (nodeindices) {
|
|
int gi_base = ci * order;
|
|
int gj_base = cj * order;
|
|
int gk_base = ck * order;
|
|
int ny_g = cy * order + 1;
|
|
int nz_g = cz * order + 1;
|
|
int ni = 0;
|
|
for (int li = 0; li <= order; li++) {
|
|
int gi = gi_base + li;
|
|
int gi_stride = gi * ny_g * nz_g;
|
|
for (int lj = 0; lj <= order; lj++) {
|
|
int gj = gj_base + lj;
|
|
int gj_stride = gi_stride + gj * nz_g;
|
|
for (int lk = 0; lk <= order; lk++) {
|
|
int gk = gk_base + lk;
|
|
nodeindices[ni++] = gj_stride + gk;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
int npc = (order + 1) * (order + 1) * (order + 1);
|
|
return npc;
|
|
}
|
|
|
|
|
|
// interpolate a function at x with given interpolation coefficients and order n
|
|
void mju_interpolate3D(mjtNum res[3], const mjtNum x[3], const mjtNum* coeff, int order,
|
|
const int* nodeindices) {
|
|
int npoint = (order + 1) * (order + 1) * (order + 1);
|
|
|
|
if (npoint > 27) {
|
|
for (int j=0; j < npoint; j++) {
|
|
int idx = nodeindices ? nodeindices[j] : j;
|
|
mju_addToScl3(res, coeff+3*idx, mju_evalBasis(x, j, order));
|
|
}
|
|
return;
|
|
}
|
|
|
|
mjtNum basis[27];
|
|
mju_evalBasisArray(basis, x, order);
|
|
|
|
for (int j=0; j < npoint; j++) {
|
|
int idx = nodeindices ? nodeindices[j] : j;
|
|
mju_addToScl3(res, coeff+3*idx, basis[j]);
|
|
}
|
|
}
|
|
|
|
|
|
static void flexInterpRotation(int order, const mjtNum* xpos_c,
|
|
const mjtNum local[3], mjtNum* quat) {
|
|
mjtNum mat[9] = {0};
|
|
|
|
if (order > 0) {
|
|
mju_defGradient(mat, local, xpos_c, order);
|
|
} else {
|
|
// order 0: fallback to identity matrix
|
|
mat[0] = 1;
|
|
mat[4] = 1;
|
|
mat[8] = 1;
|
|
}
|
|
|
|
// find rotation
|
|
quat[0] = 1;
|
|
quat[1] = 0;
|
|
quat[2] = 0;
|
|
quat[3] = 0;
|
|
mju_mat2Rot(quat, mat);
|
|
mju_negQuat(quat, quat);
|
|
}
|
|
|
|
|
|
// gather cell-local quantities and optionally compute rotation
|
|
void mju_flexGatherCellState(int order, int cy, int cz, int ci, int cj, int ck,
|
|
const mjtNum* xpos_g, const mjtNum* vel_g, const mjtNum* xpos0_g,
|
|
mjtNum* xpos_c, mjtNum* vel_c, mjtNum* xpos0_c,
|
|
int* nodeindices, mjtNum* quat) {
|
|
int ny_g = cy * order + 1;
|
|
int nz_g = cz * order + 1;
|
|
|
|
int local = 0;
|
|
for (int li = 0; li <= order; li++) {
|
|
for (int lj = 0; lj <= order; lj++) {
|
|
for (int lk = 0; lk <= order; lk++) {
|
|
int gi = ci*order + li;
|
|
int gj = cj*order + lj;
|
|
int gk = ck*order + lk;
|
|
int gidx = gi*ny_g*nz_g + gj*nz_g + gk;
|
|
|
|
if (xpos_c && xpos_g) mju_copy3(xpos_c + 3*local, xpos_g + 3*gidx);
|
|
if (vel_c && vel_g) mju_copy3(vel_c + 3*local, vel_g + 3*gidx);
|
|
if (xpos0_c && xpos0_g) mju_copy3(xpos0_c + 3*local, xpos0_g + 3*gidx);
|
|
if (nodeindices) nodeindices[local] = gidx;
|
|
|
|
local++;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (quat && xpos_c) {
|
|
mjtNum p[3] = {.5, .5, .5};
|
|
flexInterpRotation(order, xpos_c, p, quat);
|
|
}
|
|
}
|
|
|
|
|
|
// compute corotational rotation from 2D deformation gradient on a flat face
|
|
void mju_flexInterpRotation2D(int order, const mjtNum* xpos_f, int npe,
|
|
int axis0, int axis1, int normal_axis,
|
|
const mjtNum local[2], mjtNum* quat) {
|
|
// compute 3x2 deformation gradient F at parametric point local
|
|
mjtNum t1[3] = {0, 0, 0}; // tangent along axis0
|
|
mjtNum t2[3] = {0, 0, 0}; // tangent along axis1
|
|
int idx = 0;
|
|
for (int l0 = 0; l0 <= order; l0++) {
|
|
for (int l1 = 0; l1 <= order; l1++) {
|
|
mjtNum grad0 = dphi(local[0], l0, order) * phi(local[1], l1, order);
|
|
mjtNum grad1 = phi(local[0], l0, order) * dphi(local[1], l1, order);
|
|
for (int d = 0; d < 3; d++) {
|
|
t1[d] += xpos_f[3*idx + d] * grad0;
|
|
t2[d] += xpos_f[3*idx + d] * grad1;
|
|
}
|
|
idx++;
|
|
}
|
|
}
|
|
|
|
// normal = t1 x t2
|
|
mjtNum normal[3];
|
|
mju_cross(normal, t1, t2);
|
|
|
|
// build 3x3 matrix with columns assigned to canonical axes (row-major)
|
|
// axis0 → t1, axis1 → t2, normal_axis → normal
|
|
// this ensures identity rotation for axis-aligned grids
|
|
mjtNum mat[9] = {0};
|
|
mjtNum* vecs[3];
|
|
vecs[axis0] = t1;
|
|
vecs[axis1] = t2;
|
|
vecs[normal_axis] = normal;
|
|
|
|
for (int col = 0; col < 3; col++) {
|
|
mat[0*3 + col] = vecs[col][0];
|
|
mat[1*3 + col] = vecs[col][1];
|
|
mat[2*3 + col] = vecs[col][2];
|
|
}
|
|
|
|
// extract rotation via polar decomposition
|
|
quat[0] = 1;
|
|
quat[1] = 0;
|
|
quat[2] = 0;
|
|
quat[3] = 0;
|
|
mju_mat2Rot(quat, mat);
|
|
mju_negQuat(quat, quat);
|
|
}
|
|
|
|
|
|
// gather face-element-local quantities and optionally compute rotation (shell mode)
|
|
//
|
|
// face element enumeration for a grid with cell counts (cx, cy, cz):
|
|
// face 0: x=0 cy*cz quads (normal=0)
|
|
// face 1: x=max cy*cz quads (normal=0)
|
|
// face 2: y=0 cx*cz quads (normal=1)
|
|
// face 3: y=max cx*cz quads (normal=1)
|
|
// face 4: z=0 cx*cy quads (normal=2)
|
|
// face 5: z=max cx*cy quads (normal=2)
|
|
void mju_flexGatherFaceState(int order, int cx, int cy, int cz,
|
|
int face_elem_idx,
|
|
const mjtNum* xpos_g, const mjtNum* vel_g,
|
|
const mjtNum* xpos0_g,
|
|
mjtNum* xpos_f, mjtNum* vel_f, mjtNum* xpos0_f,
|
|
int* nodeindices, mjtNum* quat) {
|
|
int ny_g = cy * order + 1;
|
|
int nz_g = cz * order + 1;
|
|
int npe = (order + 1) * (order + 1);
|
|
|
|
// face sizes and properties
|
|
int face_sizes[6] = {cy*cz, cy*cz, cx*cz, cx*cz, cx*cy, cx*cy};
|
|
int face_normal[6] = {0, 0, 1, 1, 2, 2};
|
|
int face_count1[6] = {cz, cz, cx, cx, cy, cy};
|
|
int face_fixed_vals[6];
|
|
face_fixed_vals[0] = 0;
|
|
face_fixed_vals[1] = cx * order;
|
|
face_fixed_vals[2] = 0;
|
|
face_fixed_vals[3] = cy * order;
|
|
face_fixed_vals[4] = 0;
|
|
face_fixed_vals[5] = cz * order;
|
|
|
|
// determine which face and quad within face
|
|
int face_id = 0;
|
|
int within_face = face_elem_idx;
|
|
int cumul = 0;
|
|
for (int f = 0; f < 6; f++) {
|
|
if (face_elem_idx < cumul + face_sizes[f]) {
|
|
face_id = f;
|
|
within_face = face_elem_idx - cumul;
|
|
break;
|
|
}
|
|
cumul += face_sizes[f];
|
|
}
|
|
|
|
int normal_axis = face_normal[face_id];
|
|
int na0 = (normal_axis + 1) % 3; // slow in-plane axis
|
|
int na1 = (normal_axis + 2) % 3; // fast in-plane axis
|
|
int c1 = face_count1[face_id];
|
|
int g_fixed = face_fixed_vals[face_id];
|
|
int q0 = within_face / c1;
|
|
int q1 = within_face % c1;
|
|
|
|
// gather nodes
|
|
int local = 0;
|
|
for (int l0 = 0; l0 <= order; l0++) {
|
|
for (int l1 = 0; l1 <= order; l1++) {
|
|
int g[3];
|
|
g[normal_axis] = g_fixed;
|
|
g[na0] = q0 * order + l0;
|
|
g[na1] = q1 * order + l1;
|
|
int gidx = g[0] * ny_g * nz_g + g[1] * nz_g + g[2];
|
|
|
|
if (xpos_f && xpos_g) mju_copy3(xpos_f + 3*local, xpos_g + 3*gidx);
|
|
if (vel_f && vel_g) mju_copy3(vel_f + 3*local, vel_g + 3*gidx);
|
|
if (xpos0_f && xpos0_g) mju_copy3(xpos0_f + 3*local, xpos0_g + 3*gidx);
|
|
if (nodeindices) nodeindices[local] = gidx;
|
|
|
|
local++;
|
|
}
|
|
}
|
|
|
|
if (quat && xpos_f) {
|
|
mjtNum p[2] = {.5, .5};
|
|
mju_flexInterpRotation2D(order, xpos_f, npe, na0, na1, normal_axis, p, quat);
|
|
}
|
|
}
|
|
|
|
|
|
// compute unnormalized surface normal and tangent vectors at a parametric point
|
|
// on a 2D face element; normal = t1 x t2 (unnormalized)
|
|
void mju_flexFaceNormal2D(mjtNum normal[3], mjtNum t1[3], mjtNum t2[3],
|
|
int order, const mjtNum* xpos_f,
|
|
const mjtNum local[2]) {
|
|
mju_zero3(t1);
|
|
mju_zero3(t2);
|
|
int idx = 0;
|
|
for (int l0 = 0; l0 <= order; l0++) {
|
|
for (int l1 = 0; l1 <= order; l1++) {
|
|
mjtNum grad0 = dphi(local[0], l0, order) * phi(local[1], l1, order);
|
|
mjtNum grad1 = phi(local[0], l0, order) * dphi(local[1], l1, order);
|
|
for (int d = 0; d < 3; d++) {
|
|
t1[d] += xpos_f[3*idx + d] * grad0;
|
|
t2[d] += xpos_f[3*idx + d] * grad1;
|
|
}
|
|
idx++;
|
|
}
|
|
}
|
|
mju_cross(normal, t1, t2);
|
|
}
|
|
|
|
|
|
// helper: get nodexpos value for node (i,j,k) in an nx*ny*nz grid
|
|
static inline const mjtNum* nodeAt(const mjtNum* nodexpos, int ny, int nz, int i, int j, int k) {
|
|
return nodexpos + 3*(i*ny*nz + j*nz + k);
|
|
}
|
|
|
|
// reconstruct interior node positions from boundary nodes via Transfinite Interpolation
|
|
void mju_shellTrackInterior(mjtNum* nodexpos, int nx, int ny, int nz) {
|
|
// need at least 3 nodes in each direction to have interior nodes
|
|
if (nx < 3 || ny < 3 || nz < 3) {
|
|
return;
|
|
}
|
|
|
|
for (int i = 1; i < nx-1; i++) {
|
|
for (int j = 1; j < ny-1; j++) {
|
|
for (int k = 1; k < nz-1; k++) {
|
|
// parametric coordinates in [0, 1]
|
|
mjtNum s = (mjtNum)i / (nx-1);
|
|
mjtNum t = (mjtNum)j / (ny-1);
|
|
mjtNum u = (mjtNum)k / (nz-1);
|
|
|
|
mjtNum result[3] = {0, 0, 0};
|
|
|
|
// --- face contributions (bilinear interpolation on each face pair) ---
|
|
// x-faces: i=0 and i=nx-1
|
|
for (int d = 0; d < 3; d++) {
|
|
result[d] += (1-s) * nodeAt(nodexpos, ny, nz, 0, j, k)[d]
|
|
+ s * nodeAt(nodexpos, ny, nz, nx-1, j, k)[d];
|
|
}
|
|
// y-faces: j=0 and j=ny-1
|
|
for (int d = 0; d < 3; d++) {
|
|
result[d] += (1-t) * nodeAt(nodexpos, ny, nz, i, 0, k)[d]
|
|
+ t * nodeAt(nodexpos, ny, nz, i, ny-1, k)[d];
|
|
}
|
|
// z-faces: k=0 and k=nz-1
|
|
for (int d = 0; d < 3; d++) {
|
|
result[d] += (1-u) * nodeAt(nodexpos, ny, nz, i, j, 0)[d]
|
|
+ u * nodeAt(nodexpos, ny, nz, i, j, nz-1)[d];
|
|
}
|
|
|
|
// --- edge corrections (subtract 12 edges, each linearly interpolated) ---
|
|
// edges along x (4 edges: (j,k) at corners of y-z face)
|
|
for (int d = 0; d < 3; d++) {
|
|
result[d] -= (1-t)*(1-u) * nodeAt(nodexpos, ny, nz, i, 0, 0)[d];
|
|
result[d] -= (1-t)* u * nodeAt(nodexpos, ny, nz, i, 0, nz-1)[d];
|
|
result[d] -= t *(1-u) * nodeAt(nodexpos, ny, nz, i, ny-1, 0)[d];
|
|
result[d] -= t * u * nodeAt(nodexpos, ny, nz, i, ny-1, nz-1)[d];
|
|
}
|
|
// edges along y (4 edges: (i,k) at corners of x-z face)
|
|
for (int d = 0; d < 3; d++) {
|
|
result[d] -= (1-s)*(1-u) * nodeAt(nodexpos, ny, nz, 0, j, 0)[d];
|
|
result[d] -= (1-s)* u * nodeAt(nodexpos, ny, nz, 0, j, nz-1)[d];
|
|
result[d] -= s *(1-u) * nodeAt(nodexpos, ny, nz, nx-1, j, 0)[d];
|
|
result[d] -= s * u * nodeAt(nodexpos, ny, nz, nx-1, j, nz-1)[d];
|
|
}
|
|
// edges along z (4 edges: (i,j) at corners of x-y face)
|
|
for (int d = 0; d < 3; d++) {
|
|
result[d] -= (1-s)*(1-t) * nodeAt(nodexpos, ny, nz, 0, 0, k)[d];
|
|
result[d] -= (1-s)* t * nodeAt(nodexpos, ny, nz, 0, ny-1, k)[d];
|
|
result[d] -= s *(1-t) * nodeAt(nodexpos, ny, nz, nx-1, 0, k)[d];
|
|
result[d] -= s * t * nodeAt(nodexpos, ny, nz, nx-1, ny-1, k)[d];
|
|
}
|
|
|
|
// --- corner corrections (add 8 corners back) ---
|
|
for (int d = 0; d < 3; d++) {
|
|
result[d] += (1-s)*(1-t)*(1-u) * nodeAt(nodexpos, ny, nz, 0, 0, 0)[d];
|
|
result[d] += (1-s)*(1-t)* u * nodeAt(nodexpos, ny, nz, 0, 0, nz-1)[d];
|
|
result[d] += (1-s)* t *(1-u) * nodeAt(nodexpos, ny, nz, 0, ny-1, 0)[d];
|
|
result[d] += (1-s)* t * u * nodeAt(nodexpos, ny, nz, 0, ny-1, nz-1)[d];
|
|
result[d] += s *(1-t)*(1-u) * nodeAt(nodexpos, ny, nz, nx-1, 0, 0)[d];
|
|
result[d] += s *(1-t)* u * nodeAt(nodexpos, ny, nz, nx-1, 0, nz-1)[d];
|
|
result[d] += s * t *(1-u) * nodeAt(nodexpos, ny, nz, nx-1, ny-1, 0)[d];
|
|
result[d] += s * t * u * nodeAt(nodexpos, ny, nz, nx-1, ny-1, nz-1)[d];
|
|
}
|
|
|
|
// write result to interior node
|
|
mju_copy3(nodexpos + 3*(i*ny*nz + j*nz + k), result);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// helper to accumulate weights in a sparse list
|
|
static void addWeight(int* nb, int* body, mjtNum* bweight, int b, mjtNum w) {
|
|
for (int i = 0; i < *nb; i++) {
|
|
if (body[i] == b) {
|
|
if (bweight) {
|
|
bweight[i] += w;
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
body[*nb] = b;
|
|
if (bweight) {
|
|
bweight[*nb] = w;
|
|
}
|
|
(*nb)++;
|
|
}
|
|
|
|
// compute TFI weights for an interior node (i,j,k) and distribute to boundary nodes
|
|
void mju_shellTFIWeights(int nx, int ny, int nz, int i, int j, int k,
|
|
mjtNum w, int* nb, int* body, mjtNum* bweight,
|
|
const int* nodebodyid, int nstart) {
|
|
mjtNum s = (mjtNum)i / (nx-1);
|
|
mjtNum t = (mjtNum)j / (ny-1);
|
|
mjtNum u = (mjtNum)k / (nz-1);
|
|
|
|
// face contributions
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + j*nz + k], w * (1-s));
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + j*nz + k], w * s);
|
|
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + 0*nz + k], w * (1-t));
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + (ny-1)*nz + k], w * t);
|
|
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + j*nz + 0], w * (1-u));
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + j*nz + (nz-1)], w * u);
|
|
|
|
// edge corrections
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + 0*nz + 0], -w * (1-t)*(1-u));
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + 0*nz + (nz-1)], -w * (1-t)*u);
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + (ny-1)*nz + 0], -w * t*(1-u));
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + (ny-1)*nz + (nz-1)], -w * t*u);
|
|
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + j*nz + 0], -w * (1-s)*(1-u));
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + j*nz + (nz-1)], -w * (1-s)*u);
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + j*nz + 0], -w * s*(1-u));
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + j*nz + (nz-1)], -w * s*u);
|
|
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + 0*nz + k], -w * (1-s)*(1-t));
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + (ny-1)*nz + k], -w * (1-s)*t);
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + 0*nz + k], -w * s*(1-t));
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + (ny-1)*nz + k], -w * s*t);
|
|
|
|
// corner corrections
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + 0*nz + 0], w * (1-s)*(1-t)*(1-u));
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + 0*nz + (nz-1)], w * (1-s)*(1-t)*u);
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + (ny-1)*nz + 0], w * (1-s)*t*(1-u));
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + (ny-1)*nz + (nz-1)], w * (1-s)*t*u);
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + 0*nz + 0], w * s*(1-t)*(1-u));
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + 0*nz + (nz-1)], w * s*(1-t)*u);
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + (ny-1)*nz + 0], w * s*t*(1-u));
|
|
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + (ny-1)*nz + (nz-1)], w * s*t*u);
|
|
}
|
|
|
|
|
|
//------------------------------ actuator models ---------------------------------------------------
|
|
|
|
// normalized muscle length-gain curve
|
|
mjtNum mju_muscleGainLength(mjtNum length, mjtNum lmin, mjtNum lmax) {
|
|
if (lmin <= length && length <= lmax) {
|
|
// mid-ranges (maximum is at 1.0)
|
|
mjtNum a = 0.5*(lmin+1);
|
|
mjtNum b = 0.5*(1+lmax);
|
|
|
|
if (length <= a) {
|
|
mjtNum x = (length-lmin) / mjMAX(mjMINVAL, a-lmin);
|
|
return 0.5*x*x;
|
|
} else if (length <= 1) {
|
|
mjtNum x = (1-length) / mjMAX(mjMINVAL, 1-a);
|
|
return 1 - 0.5*x*x;
|
|
} else if (length <= b) {
|
|
mjtNum x = (length-1) / mjMAX(mjMINVAL, b-1);
|
|
return 1 - 0.5*x*x;
|
|
} else {
|
|
mjtNum x = (lmax-length) / mjMAX(mjMINVAL, lmax-b);
|
|
return 0.5*x*x;
|
|
}
|
|
}
|
|
|
|
return 0.0;
|
|
}
|
|
|
|
|
|
// muscle active force, prm = (range[2], force, scale, lmin, lmax, vmax, fpmax, fvmax)
|
|
mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2],
|
|
mjtNum acc0, const mjtNum prm[9]) {
|
|
// unpack parameters
|
|
mjtNum range[2] = {prm[0], prm[1]};
|
|
mjtNum force = prm[2];
|
|
mjtNum scale = prm[3];
|
|
mjtNum lmin = prm[4];
|
|
mjtNum lmax = prm[5];
|
|
mjtNum vmax = prm[6];
|
|
mjtNum fvmax = prm[8];
|
|
|
|
// scale force if negative
|
|
if (force < 0) {
|
|
force = scale / mjMAX(mjMINVAL, acc0);
|
|
}
|
|
|
|
// optimum length
|
|
mjtNum L0 = (lengthrange[1]-lengthrange[0]) / mjMAX(mjMINVAL, range[1]-range[0]);
|
|
|
|
// normalized length and velocity
|
|
mjtNum L = range[0] + (len-lengthrange[0]) / mjMAX(mjMINVAL, L0);
|
|
mjtNum V = vel / mjMAX(mjMINVAL, L0*vmax);
|
|
|
|
// length curve
|
|
mjtNum FL = mju_muscleGainLength(L, lmin, lmax);
|
|
|
|
// velocity curve
|
|
mjtNum FV;
|
|
mjtNum y = fvmax-1;
|
|
if (V <= -1) {
|
|
FV = 0;
|
|
} else if (V <= 0) {
|
|
FV = (V+1)*(V+1);
|
|
} else if (V <= y) {
|
|
FV = fvmax - (y-V)*(y-V) / mjMAX(mjMINVAL, y);
|
|
} else {
|
|
FV = fvmax;
|
|
}
|
|
|
|
// compute FVL and scale, make it negative
|
|
return -force*FL*FV;
|
|
}
|
|
|
|
|
|
// muscle passive force, prm = (range[2], force, scale, lmin, lmax, vmax, fpmax, fvmax)
|
|
mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2],
|
|
mjtNum acc0, const mjtNum prm[9]) {
|
|
// unpack parameters
|
|
mjtNum range[2] = {prm[0], prm[1]};
|
|
mjtNum force = prm[2];
|
|
mjtNum scale = prm[3];
|
|
mjtNum lmax = prm[5];
|
|
mjtNum fpmax = prm[7];
|
|
|
|
// scale force if negative
|
|
if (force < 0) {
|
|
force = scale / mjMAX(mjMINVAL, acc0);
|
|
}
|
|
|
|
// optimum length
|
|
mjtNum L0 = (lengthrange[1]-lengthrange[0]) / mjMAX(mjMINVAL, range[1]-range[0]);
|
|
|
|
// normalized length
|
|
mjtNum L = range[0] + (len-lengthrange[0]) / mjMAX(mjMINVAL, L0);
|
|
|
|
// half-quadratic to (L0+lmax)/2, linear beyond
|
|
mjtNum b = 0.5*(1+lmax);
|
|
if (L <= 1) {
|
|
return 0;
|
|
} else if (L <= b) {
|
|
mjtNum x = (L-1) / mjMAX(mjMINVAL, b-1);
|
|
return -force*fpmax*0.5*x*x;
|
|
} else {
|
|
mjtNum x = (L-b) / mjMAX(mjMINVAL, b-1);
|
|
return -force*fpmax*(0.5 + x);
|
|
}
|
|
}
|
|
|
|
|
|
// muscle time constant with optional smoothing
|
|
mjtNum mju_muscleDynamicsTimescale(mjtNum dctrl, mjtNum tau_act, mjtNum tau_deact,
|
|
mjtNum smoothing_width) {
|
|
mjtNum tau;
|
|
|
|
// hard switching
|
|
if (smoothing_width < mjMINVAL) {
|
|
tau = dctrl > 0 ? tau_act : tau_deact;
|
|
}
|
|
|
|
// smooth switching
|
|
else {
|
|
// scale by width, center around 0.5 midpoint, rescale to bounds
|
|
tau = tau_deact + (tau_act-tau_deact)*mju_sigmoid(dctrl/smoothing_width + 0.5);
|
|
}
|
|
return tau;
|
|
}
|
|
|
|
|
|
// muscle activation dynamics, prm = (tau_act, tau_deact, smoothing_width)
|
|
mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]) {
|
|
// clamp control
|
|
mjtNum ctrlclamp = mju_clip(ctrl, 0, 1);
|
|
|
|
// clamp activation
|
|
mjtNum actclamp = mju_clip(act, 0, 1);
|
|
|
|
// compute timescales as in Millard et al. (2013) https://doi.org/10.1115/1.4023390
|
|
mjtNum tau_act = prm[0] * (0.5 + 1.5*actclamp); // activation timescale
|
|
mjtNum tau_deact = prm[1] / (0.5 + 1.5*actclamp); // deactivation timescale
|
|
mjtNum smoothing_width = prm[2]; // width of smoothing sigmoid
|
|
mjtNum dctrl = ctrlclamp - act; // excess excitation
|
|
|
|
mjtNum tau = mju_muscleDynamicsTimescale(dctrl, tau_act, tau_deact, smoothing_width);
|
|
|
|
// filter output
|
|
return dctrl / mjMAX(mjMINVAL, tau);
|
|
}
|
|
|
|
|
|
// LuGre Stribeck function: g(v) = F_C + (F_S - F_C) * exp(-(v/v_S)^2)
|
|
mjtNum mj_lugreStribeck(mjtNum velocity, mjtNum F_C, mjtNum F_S, mjtNum v_S) {
|
|
mjtNum ratio = velocity / mju_max(mjMINVAL, v_S);
|
|
return F_C + (F_S - F_C) * mju_exp(-ratio*ratio);
|
|
}
|
|
|
|
|
|
// compute DC motor activation slot indices from parameter arrays
|
|
mjDCMotorSlots mj_dcmotorSlots(const mjtNum* dynprm, const mjtNum* gainprm) {
|
|
mjDCMotorSlots s = {-1, -1, -1, -1, -1, 0};
|
|
if (dynprm[7] > 0) s.slew = s.num_slots++; // slew rate limiting
|
|
if (gainprm[5] > 0) s.integral = s.num_slots++; // PI integral
|
|
if (dynprm[2] > 0) s.temperature = s.num_slots++; // thermal model
|
|
if (dynprm[5] > 0) s.bristle = s.num_slots++; // LuGre bristle
|
|
if (dynprm[0] > 0) s.current = s.num_slots++; // current filter
|
|
|
|
return s;
|
|
}
|
|
|
|
|
|
//---------------------------------------- Base64 --------------------------------------------------
|
|
|
|
// decoding function for Base64
|
|
static uint32_t _decode(char ch) {
|
|
if (ch >= 'A' && ch <= 'Z') {
|
|
return ch - 'A';
|
|
}
|
|
|
|
if (ch >= 'a' && ch <= 'z') {
|
|
return (ch - 'a') + 26;
|
|
}
|
|
|
|
if (ch >= '0' && ch <= '9') {
|
|
return (ch - '0') + 52;
|
|
}
|
|
|
|
if (ch == '+') {
|
|
return 62;
|
|
}
|
|
|
|
if (ch == '/') {
|
|
return 63;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
// encode data as Base64 into buf (including padding and null char)
|
|
// returns number of chars written in buf: 4 * [(ndata + 2) / 3] + 1
|
|
size_t mju_encodeBase64(char* buf, const uint8_t* data, size_t ndata) {
|
|
static const char *table =
|
|
"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
|
|
|
|
int i = 0, j = 0;
|
|
|
|
// loop over 24 bit chunks
|
|
while (i + 3 <= ndata) {
|
|
// take next 24 bit chunk (3 bytes)
|
|
uint32_t byte_1 = data[i++];
|
|
uint32_t byte_2 = data[i++];
|
|
uint32_t byte_3 = data[i++];
|
|
|
|
// merge bytes into one 32 bit int
|
|
uint32_t k = (byte_1 << 16) | (byte_2 << 8) | byte_3;
|
|
|
|
// encode 6 bit chucks into four chars
|
|
buf[j++] = table[(k >> 18) & 63];
|
|
buf[j++] = table[(k >> 12) & 63];
|
|
buf[j++] = table[(k >> 6) & 63];
|
|
buf[j++] = table[(k >> 0) & 63];
|
|
}
|
|
|
|
// one byte left
|
|
if (i + 1 == ndata) {
|
|
uint32_t byte_1 = data[i];
|
|
uint32_t k = byte_1 << 16;
|
|
buf[j++] = table[(k >> 18) & 63];
|
|
buf[j++] = table[(k >> 12) & 63];
|
|
buf[j++] = '='; // padding
|
|
buf[j++] = '='; // padding
|
|
}
|
|
|
|
// two bytes left
|
|
if (i + 2 == ndata) {
|
|
uint32_t byte_1 = data[i++];
|
|
uint32_t byte_2 = data[i];
|
|
|
|
uint32_t k = (byte_1 << 16) + (byte_2 << 8);
|
|
|
|
buf[j++] = table[(k >> 18) & 63];
|
|
buf[j++] = table[(k >> 12) & 63];
|
|
buf[j++] = table[(k >> 6) & 63];
|
|
buf[j++] = '='; // padding
|
|
}
|
|
|
|
buf[j] = '\0';
|
|
return 4 * ((ndata + 2) / 3) + 1;
|
|
}
|
|
|
|
|
|
// return size in decoded bytes if s is a valid Base64 encoding
|
|
// return 0 if s is empty or invalid Base64 encoding
|
|
size_t mju_isValidBase64(const char* s) {
|
|
size_t i = 0;
|
|
int pad = 0; // 0, 1, or 2 zero padding at the end of s
|
|
|
|
// validate chars
|
|
for (; s[i] && s[i] != '='; i++) {
|
|
if (!isalnum(s[i]) && s[i] != '/' && s[i] != '+') {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
// padding at end
|
|
if (s[i] == '=') {
|
|
if (!s[i + 1]) {
|
|
pad = 1; // one '=' padding at end
|
|
} else if (s[i + 1] == '=' && !s[i + 2]) {
|
|
pad = 2; // two '=' padding at end
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
// strlen(s) must be a multiple of 4
|
|
int len = i + pad;
|
|
return len % 4 ? 0 : 3 * (len / 4) - pad;
|
|
}
|
|
|
|
|
|
// decode valid Base64 in string s into buf, undefined behavior if s is not valid Base64
|
|
// returns number of bytes decoded (upper limit of 3 * (strlen(s) / 4))
|
|
size_t mju_decodeBase64(uint8_t* buf, const char* s) {
|
|
size_t i = 0, j = 0;
|
|
|
|
// loop over 24 bit chunks
|
|
while (s[i] != '\0') {
|
|
// take next 24 bit chuck (4 chars; 6 bits each)
|
|
uint32_t char_1 = _decode(s[i++]);
|
|
uint32_t char_2 = _decode(s[i++]);
|
|
uint32_t char_3 = _decode(s[i++]);
|
|
uint32_t char_4 = _decode(s[i++]);
|
|
|
|
// merge into 32 bit int
|
|
uint32_t k = (char_1 << 18) | (char_2 << 12) | (char_3 << 6) | char_4;
|
|
|
|
|
|
// write up to three bytes (exclude padding at end)
|
|
buf[j++] = (k >> 16) & 0xFF;
|
|
if (s[i - 2] != '=') {
|
|
buf[j++] = (k >> 8) & 0xFF;
|
|
}
|
|
if (s[i - 1] != '=') {
|
|
buf[j++] = k & 0xFF;
|
|
}
|
|
}
|
|
return j;
|
|
}
|
|
|
|
|
|
//------------------------------ history buffers ---------------------------------------------------
|
|
|
|
// convert logical index (0=oldest, n-1=newest) to physical index
|
|
// cursor points to the newest element (logical index n-1)
|
|
static inline int historyPhysicalIndex(int cursor, int n, int logical) {
|
|
return (cursor + 1 + logical) % n;
|
|
}
|
|
|
|
|
|
// find logical index i such that times[i-1] < t <= times[i], using circular binary search
|
|
// returns 0 if t <= times[oldest], n if t > times[newest]
|
|
// cursor points to the newest element (logical index n-1)
|
|
static int historyFindIndex(const mjtNum* times, int n, int cursor, mjtNum t) {
|
|
// get oldest and newest timestamps
|
|
int oldest_phys = historyPhysicalIndex(cursor, n, 0);
|
|
int newest_phys = historyPhysicalIndex(cursor, n, n-1);
|
|
mjtNum t_oldest = times[oldest_phys];
|
|
mjtNum t_newest = times[newest_phys];
|
|
|
|
// before or at first element
|
|
if (t <= t_oldest) {
|
|
return 0;
|
|
}
|
|
|
|
// after last element
|
|
if (t > t_newest) {
|
|
return n;
|
|
}
|
|
|
|
// circular binary search: find smallest logical i such that times[phys(i)] >= t
|
|
int lo = 0;
|
|
int hi = n - 1;
|
|
while (hi - lo > 1) {
|
|
int mid = (lo + hi) / 2;
|
|
int mid_phys = historyPhysicalIndex(cursor, n, mid);
|
|
if (times[mid_phys] < t) {
|
|
lo = mid;
|
|
} else {
|
|
hi = mid;
|
|
}
|
|
}
|
|
|
|
return hi;
|
|
}
|
|
|
|
|
|
// initialize history buffer with given times and values; times must be strictly increasing
|
|
// buffer layout: [user(1), cursor(1), times(n), values(n*dim)]
|
|
void mju_historyInit(mjtNum* buf, int n, int dim, const mjtNum* times, const mjtNum* values,
|
|
mjtNum user) {
|
|
// check strict monotonicity of times
|
|
for (int i = 0; i < n-1; i++) {
|
|
if (times[i+1] - times[i] < mjMINVAL) {
|
|
mjERROR("times must be strictly increasing, got times[%d]=%g >= times[%d]=%g",
|
|
i, times[i], i+1, times[i+1]);
|
|
}
|
|
}
|
|
|
|
// buf layout: [user(1), cursor(1), times(n), values(n*dim)]
|
|
buf[0] = user; // user value
|
|
buf[1] = (mjtNum)(n-1); // cursor points to newest (logical index n-1 = physical index n-1)
|
|
|
|
mjtNum* buf_times = buf + 2;
|
|
mjtNum* buf_values = buf + 2 + n;
|
|
|
|
if (times != buf_times) mju_copy(buf_times, times, n);
|
|
if (values) mju_copy(buf_values, values, n*dim);
|
|
}
|
|
|
|
|
|
// find insertion slot for time t, maintaining sorted order
|
|
// if t matches an existing timestamp, returns pointer to that slot
|
|
// if a new sample is inserted, the oldest sample is dropped
|
|
// returns pointer to value slot where caller should write dim values
|
|
mjtNum* mju_historyInsert(mjtNum* buf, int n, int dim, mjtNum t) {
|
|
int cursor = (int)buf[1];
|
|
mjtNum* times = buf + 2;
|
|
mjtNum* values = buf + 2 + n;
|
|
|
|
// find logical insertion index: times[i-1] < t <= times[i]
|
|
int i = historyFindIndex(times, n, cursor, t);
|
|
|
|
// exact match at logical i: return pointer to existing slot
|
|
if (i < n) {
|
|
int phys_i = historyPhysicalIndex(cursor, n, i);
|
|
if (mju_abs(t - times[phys_i]) < mjMINVAL) {
|
|
return values + phys_i*dim;
|
|
}
|
|
}
|
|
|
|
// logical i == 0: new sample is older than oldest, replace oldest slot
|
|
if (i == 0) {
|
|
int oldest_phys = historyPhysicalIndex(cursor, n, 0);
|
|
times[oldest_phys] = t;
|
|
return values + oldest_phys*dim;
|
|
}
|
|
|
|
// logical i == n: new sample is newer than newest, advance cursor and write
|
|
if (i == n) {
|
|
cursor = (cursor + 1) % n;
|
|
buf[1] = (mjtNum)cursor;
|
|
|
|
// cursor now points to the new newest slot (which was the old oldest)
|
|
times[cursor] = t;
|
|
return values + cursor*dim;
|
|
}
|
|
|
|
// 0 < i < n: out-of-order insertion, shift [1, i-1] left (dropping 0), insert at i-1
|
|
for (int j = 0; j < i-1; j++) {
|
|
int src_phys = historyPhysicalIndex(cursor, n, j+1);
|
|
int dst_phys = historyPhysicalIndex(cursor, n, j);
|
|
times[dst_phys] = times[src_phys];
|
|
mju_copy(values + dst_phys*dim, values + src_phys*dim, dim);
|
|
}
|
|
int insert_phys = historyPhysicalIndex(cursor, n, i-1);
|
|
times[insert_phys] = t;
|
|
return values + insert_phys*dim;
|
|
}
|
|
|
|
|
|
// read vector value at time t; interp: 0=zero-order-hold, 1=linear, 2=cubic spline
|
|
// returns pointer to sample in buffer on exact match or ZOH (res untouched)
|
|
// returns NULL and writes interpolated result to res on interpolation
|
|
const mjtNum* mju_historyRead(const mjtNum* buf, int n, int dim, mjtNum* res, mjtNum t, int interp) {
|
|
int cursor = (int)buf[1];
|
|
const mjtNum* times = buf + 2;
|
|
const mjtNum* values = buf + 2 + n;
|
|
|
|
int oldest_phys = historyPhysicalIndex(cursor, n, 0);
|
|
int newest_phys = historyPhysicalIndex(cursor, n, n-1);
|
|
mjtNum t_oldest = times[oldest_phys];
|
|
mjtNum t_newest = times[newest_phys];
|
|
|
|
// extrapolate before oldest: return pointer to oldest value
|
|
if (t <= t_oldest + mjMINVAL) {
|
|
return values + oldest_phys*dim;
|
|
}
|
|
|
|
// extrapolate after newest: return pointer to newest value
|
|
if (t >= t_newest - mjMINVAL) {
|
|
return values + newest_phys*dim;
|
|
}
|
|
|
|
// find bracketing logical index: times[i-1] < t <= times[i]
|
|
int i = historyFindIndex(times, n, cursor, t);
|
|
int phys_i = historyPhysicalIndex(cursor, n, i);
|
|
|
|
// check for exact match at i
|
|
if (mju_abs(t - times[phys_i]) < mjMINVAL) {
|
|
return values + phys_i*dim;
|
|
}
|
|
|
|
// lo = i-1, hi = i (we know i > 0 because t > t_oldest)
|
|
int phys_lo = historyPhysicalIndex(cursor, n, i-1);
|
|
int phys_hi = phys_i;
|
|
|
|
// zero-order hold: return pointer to lo (most recent sample <= t)
|
|
if (interp == 0) {
|
|
return values + phys_lo*dim;
|
|
}
|
|
|
|
mjtNum dt = times[phys_hi] - times[phys_lo];
|
|
mjtNum alpha = (t - times[phys_lo]) / dt;
|
|
|
|
// piecewise linear interpolation
|
|
if (interp == 1) {
|
|
for (int d = 0; d < dim; d++) {
|
|
res[d] = values[phys_lo*dim+d] + alpha * (values[phys_hi*dim+d] - values[phys_lo*dim+d]);
|
|
}
|
|
}
|
|
|
|
// cubic spline interpolation
|
|
else {
|
|
// Hermite basis functions
|
|
mjtNum alpha2 = alpha * alpha;
|
|
mjtNum alpha3 = alpha2 * alpha;
|
|
mjtNum h00 = 2*alpha3 - 3*alpha2 + 1;
|
|
mjtNum h10 = alpha3 - 2*alpha2 + alpha;
|
|
mjtNum h01 = -2*alpha3 + 3*alpha2;
|
|
mjtNum h11 = alpha3 - alpha2;
|
|
|
|
for (int d = 0; d < dim; d++) {
|
|
// finite differenced catmull-rom slopes, 0 at endpoints (constant extrapolation)
|
|
|
|
mjtNum m_lo = 0;
|
|
if (i > 1) {
|
|
int phys_lo_prev = historyPhysicalIndex(cursor, n, i-2);
|
|
mjtNum dt_lo = times[phys_hi] - times[phys_lo_prev];
|
|
m_lo = (values[phys_hi*dim+d] - values[phys_lo_prev*dim+d]) / dt_lo;
|
|
}
|
|
|
|
mjtNum m_hi = 0;
|
|
if (i < n - 1) {
|
|
int phys_hi_next = historyPhysicalIndex(cursor, n, i+1);
|
|
mjtNum dt_hi = times[phys_hi_next] - times[phys_lo];
|
|
m_hi = (values[phys_hi_next*dim+d] - values[phys_lo*dim+d]) / dt_hi;
|
|
}
|
|
|
|
res[d] = h00 * values[phys_lo*dim+d] +
|
|
h10 * dt * m_lo +
|
|
h01 * values[phys_hi*dim+d] +
|
|
h11 * dt * m_hi;
|
|
}
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
|
|
//------------------------------ miscellaneous -----------------------------------------------------
|
|
|
|
// convert contact force to pyramid representation
|
|
// the pyramid frame is: V0_i = N + mu_i*T_i
|
|
// V1_i = N - mu_i*T_i
|
|
void mju_encodePyramid(mjtNum* pyramid, const mjtNum* force, const mjtNum* mu, int dim) {
|
|
mjtNum a = force[0]/(dim-1), b;
|
|
|
|
// arbitrary redundancy resolution:
|
|
// pyramid0_i + pyramid1_i = force_normal/(dim-1) = a
|
|
// pyramid0_i - pyramid1_i = force_tangent_i/mu_i = b
|
|
for (int i=0; i < dim-1; i++) {
|
|
b = mju_min(a, force[i+1]/mu[i]);
|
|
pyramid[2*i] = 0.5*(a+b);
|
|
pyramid[2*i+1] = 0.5*(a-b);
|
|
}
|
|
}
|
|
|
|
|
|
// convert pyramid representation to contact force
|
|
void mju_decodePyramid(mjtNum* force, const mjtNum* pyramid, const mjtNum* mu, int dim) {
|
|
// special handling of frictionless contacts
|
|
if (dim == 1) {
|
|
force[0] = pyramid[0];
|
|
return;
|
|
}
|
|
|
|
// force_normal = sum(pyramid0_i + pyramid1_i)
|
|
force[0] = 0;
|
|
for (int i=0; i < 2*(dim-1); i++) {
|
|
force[0] += pyramid[i];
|
|
}
|
|
|
|
// force_tangent_i = (pyramid0_i - pyramid1_i) * mu_i
|
|
for (int i=0; i < dim-1; i++) {
|
|
force[i+1] = (pyramid[2*i] - pyramid[2*i+1]) * mu[i];
|
|
}
|
|
}
|
|
|
|
|
|
// integrate spring-damper analytically, return pos(t)
|
|
mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum k, mjtNum b, mjtNum t) {
|
|
mjtNum det, c1, c2, r1, r2, w;
|
|
|
|
// determinant of characteristic equation
|
|
det = b*b - 4*k;
|
|
|
|
// overdamping
|
|
// pos(t) = c1*exp(r1*t) + c2*exp(r2*t); r12 = (-b +- sqrt(det))/2
|
|
if (det > mjMINVAL) {
|
|
// compute w = sqrt(det)/2
|
|
w = mju_sqrt(det)/2;
|
|
|
|
// compute r1,r2
|
|
r1 = -b/2 + w;
|
|
r2 = -b/2 - w;
|
|
|
|
// compute coefficients
|
|
c1 = (pos0*r2-vel0) / (r2-r1);
|
|
c2 = (pos0*r1-vel0) / (r1-r2);
|
|
|
|
// evaluate result
|
|
return c1*mju_exp(r1*t) + c2*mju_exp(r2*t);
|
|
}
|
|
|
|
// critical damping
|
|
// pos(t) = exp(-b*t/2) * (c1 + c2*t)
|
|
else if (det <= mjMINVAL && det >= -mjMINVAL) {
|
|
// compute coefficients
|
|
c1 = pos0;
|
|
c2 = vel0 + b*c1/2;
|
|
|
|
// evaluate result
|
|
return mju_exp(-b*t/2) * (c1 + c2*t);
|
|
}
|
|
|
|
// underdamping
|
|
// pos(t) = exp(-b*t/2) * (c1*cos(w*t) + c2*sin(w*t)); w = sqrt(abs(det))/2
|
|
else {
|
|
// compute w
|
|
w = mju_sqrt(mju_abs(det))/2;
|
|
|
|
// compute coefficients
|
|
c1 = pos0;
|
|
c2 = (vel0 + b*c1/2)/w;
|
|
|
|
// evaluate result
|
|
return mju_exp(-b*t/2) * (c1*mju_cos(w*t) + c2*mju_sin(w*t));
|
|
}
|
|
}
|
|
|
|
|
|
// return 1 if point is outside box given by pos, mat, size * inflate
|
|
// return -1 if point is inside box given by pos, mat, size / inflate
|
|
// return 0 if point is between the inflated and deflated boxes
|
|
int mju_outsideBox(const mjtNum point[3], const mjtNum pos[3], const mjtNum mat[9],
|
|
const mjtNum size[3], mjtNum inflate) {
|
|
// check inflation coefficient
|
|
if (inflate < 1) {
|
|
mjERROR("inflation coefficient must be >= 1")
|
|
}
|
|
|
|
// vector from pos to point, projected to box frame
|
|
mjtNum vec[3] = {point[0]-pos[0], point[1]-pos[1], point[2]-pos[2]};
|
|
mju_mulMatTVec3(vec, mat, vec);
|
|
|
|
// big: inflated box
|
|
mjtNum big[3] = {size[0], size[1], size[2]};
|
|
if (inflate > 1) {
|
|
mju_scl3(big, big, inflate);
|
|
}
|
|
|
|
// check if outside big box
|
|
if (vec[0] > big[0] || vec[0] < -big[0] ||
|
|
vec[1] > big[1] || vec[1] < -big[1] ||
|
|
vec[2] > big[2] || vec[2] < -big[2]) {
|
|
return 1;
|
|
}
|
|
|
|
// quick return if no inflation
|
|
if (inflate == 1) {
|
|
return -1;
|
|
}
|
|
|
|
// check if inside small (deflated) box
|
|
mjtNum small[3] = {size[0]/inflate, size[1]/inflate, size[2]/inflate};
|
|
if (vec[0] < small[0] && vec[0] > -small[0] &&
|
|
vec[1] < small[1] && vec[1] > -small[1] &&
|
|
vec[2] < small[2] && vec[2] > -small[2]) {
|
|
return -1;
|
|
}
|
|
|
|
// within margin between small and big box
|
|
return 0;
|
|
}
|
|
|
|
|
|
// print matrix to screen
|
|
void mju_printMat(const mjtNum* mat, int nr, int nc) {
|
|
for (int r=0; r < nr; r++) {
|
|
for (int c=0; c < nc; c++) {
|
|
printf("%.8f ", mat[r*nc+c]);
|
|
}
|
|
printf("\n");
|
|
}
|
|
printf("\n");
|
|
}
|
|
|
|
|
|
// print sparse matrix to screen
|
|
void mju_printMatSparse(const mjtNum* mat, int nr,
|
|
const int* rownnz, const int* rowadr,
|
|
const int* colind) {
|
|
for (int r=0; r < nr; r++) {
|
|
for (int adr=rowadr[r]; adr < rowadr[r]+rownnz[r]; adr++) {
|
|
printf("(%d %d): %9.6f ", r, colind[adr], mat[adr]);
|
|
}
|
|
printf("\n");
|
|
}
|
|
printf("\n");
|
|
}
|
|
|
|
|
|
// min function, avoid re-evaluation
|
|
mjtNum mju_min(mjtNum a, mjtNum b) {
|
|
return a <= b ? a : b;
|
|
}
|
|
|
|
|
|
// max function, avoid re-evaluation
|
|
mjtNum mju_max(mjtNum a, mjtNum b) {
|
|
return a >= b ? a : b;
|
|
}
|
|
|
|
|
|
// clip x to the range [min, max]
|
|
mjtNum mju_clip(mjtNum x, mjtNum min, mjtNum max) {
|
|
if (x < min) {
|
|
return min;
|
|
} else if (x > max) {
|
|
return max;
|
|
} else {
|
|
return x;
|
|
}
|
|
}
|
|
|
|
|
|
// sign function
|
|
mjtNum mju_sign(mjtNum x) {
|
|
if (x < 0) {
|
|
return -1;
|
|
} else if (x > 0) {
|
|
return 1;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
|
|
// round to nearest integer
|
|
int mju_round(mjtNum x) {
|
|
mjtNum lower = floor(x);
|
|
mjtNum upper = ceil(x);
|
|
|
|
if (x-lower < upper-x) {
|
|
return (int)lower;
|
|
} else {
|
|
return (int)upper;
|
|
}
|
|
}
|
|
|
|
|
|
// convert type id to type name
|
|
const char* mju_type2Str(int type) {
|
|
switch ((mjtObj) type) {
|
|
case mjOBJ_BODY:
|
|
return "body";
|
|
|
|
case mjOBJ_XBODY:
|
|
return "xbody";
|
|
|
|
case mjOBJ_JOINT:
|
|
return "joint";
|
|
|
|
case mjOBJ_DOF:
|
|
return "dof";
|
|
|
|
case mjOBJ_GEOM:
|
|
return "geom";
|
|
|
|
case mjOBJ_SITE:
|
|
return "site";
|
|
|
|
case mjOBJ_CAMERA:
|
|
return "camera";
|
|
|
|
case mjOBJ_LIGHT:
|
|
return "light";
|
|
|
|
case mjOBJ_FLEX:
|
|
return "flex";
|
|
|
|
case mjOBJ_MESH:
|
|
return "mesh";
|
|
|
|
case mjOBJ_SKIN:
|
|
return "skin";
|
|
|
|
case mjOBJ_HFIELD:
|
|
return "hfield";
|
|
|
|
case mjOBJ_TEXTURE:
|
|
return "texture";
|
|
|
|
case mjOBJ_MATERIAL:
|
|
return "material";
|
|
|
|
case mjOBJ_PAIR:
|
|
return "pair";
|
|
|
|
case mjOBJ_EXCLUDE:
|
|
return "exclude";
|
|
|
|
case mjOBJ_EQUALITY:
|
|
return "equality";
|
|
|
|
case mjOBJ_TENDON:
|
|
return "tendon";
|
|
|
|
case mjOBJ_ACTUATOR:
|
|
return "actuator";
|
|
|
|
case mjOBJ_SENSOR:
|
|
return "sensor";
|
|
|
|
case mjOBJ_NUMERIC:
|
|
return "numeric";
|
|
|
|
case mjOBJ_TEXT:
|
|
return "text";
|
|
|
|
case mjOBJ_TUPLE:
|
|
return "tuple";
|
|
|
|
case mjOBJ_KEY:
|
|
return "key";
|
|
|
|
case mjOBJ_PLUGIN:
|
|
return "plugin";
|
|
|
|
case mjOBJ_FRAME:
|
|
return "frame";
|
|
|
|
default:
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
|
|
// convert type id to type name
|
|
int mju_str2Type(const char* str) {
|
|
if (!strcmp(str, "body")) {
|
|
return mjOBJ_BODY;
|
|
}
|
|
|
|
else if (!strcmp(str, "xbody")) {
|
|
return mjOBJ_XBODY;
|
|
}
|
|
|
|
else if (!strcmp(str, "joint")) {
|
|
return mjOBJ_JOINT;
|
|
}
|
|
|
|
else if (!strcmp(str, "dof")) {
|
|
return mjOBJ_DOF;
|
|
}
|
|
|
|
else if (!strcmp(str, "geom")) {
|
|
return mjOBJ_GEOM;
|
|
}
|
|
|
|
else if (!strcmp(str, "site")) {
|
|
return mjOBJ_SITE;
|
|
}
|
|
|
|
else if (!strcmp(str, "camera")) {
|
|
return mjOBJ_CAMERA;
|
|
}
|
|
|
|
else if (!strcmp(str, "light")) {
|
|
return mjOBJ_LIGHT;
|
|
}
|
|
|
|
else if (!strcmp(str, "flex")) {
|
|
return mjOBJ_FLEX;
|
|
}
|
|
|
|
else if (!strcmp(str, "mesh")) {
|
|
return mjOBJ_MESH;
|
|
}
|
|
|
|
else if (!strcmp(str, "skin")) {
|
|
return mjOBJ_SKIN;
|
|
}
|
|
|
|
else if (!strcmp(str, "hfield")) {
|
|
return mjOBJ_HFIELD;
|
|
}
|
|
|
|
else if (!strcmp(str, "texture")) {
|
|
return mjOBJ_TEXTURE;
|
|
}
|
|
|
|
else if (!strcmp(str, "material")) {
|
|
return mjOBJ_MATERIAL;
|
|
}
|
|
|
|
else if (!strcmp(str, "pair")) {
|
|
return mjOBJ_PAIR;
|
|
}
|
|
|
|
else if (!strcmp(str, "exclude")) {
|
|
return mjOBJ_EXCLUDE;
|
|
}
|
|
|
|
else if (!strcmp(str, "equality")) {
|
|
return mjOBJ_EQUALITY;
|
|
}
|
|
|
|
else if (!strcmp(str, "tendon")) {
|
|
return mjOBJ_TENDON;
|
|
}
|
|
|
|
else if (!strcmp(str, "actuator")) {
|
|
return mjOBJ_ACTUATOR;
|
|
}
|
|
|
|
else if (!strcmp(str, "sensor")) {
|
|
return mjOBJ_SENSOR;
|
|
}
|
|
|
|
else if (!strcmp(str, "numeric")) {
|
|
return mjOBJ_NUMERIC;
|
|
}
|
|
|
|
else if (!strcmp(str, "text")) {
|
|
return mjOBJ_TEXT;
|
|
}
|
|
|
|
else if (!strcmp(str, "tuple")) {
|
|
return mjOBJ_TUPLE;
|
|
}
|
|
|
|
else if (!strcmp(str, "key")) {
|
|
return mjOBJ_KEY;
|
|
}
|
|
|
|
else if (!strcmp(str, "plugin")) {
|
|
return mjOBJ_PLUGIN;
|
|
}
|
|
|
|
else {
|
|
return mjOBJ_UNKNOWN;
|
|
}
|
|
}
|
|
|
|
|
|
// return human readable number of bytes using standard letter suffix
|
|
const char* mju_writeNumBytes(size_t nbytes) {
|
|
int i;
|
|
static mjTHREADLOCAL char message[20];
|
|
static const char suffix[] = " KMGTPE";
|
|
for (i=0; i < 6; i++) {
|
|
const size_t bits = (size_t)(1) << (10*(6-i));
|
|
if (nbytes >= bits && !(nbytes & (bits - 1))) {
|
|
break;
|
|
}
|
|
}
|
|
if (i < 6) {
|
|
mjSNPRINTF(message, "%zu%c", nbytes >> (10*(6-i)), suffix[6-i]);
|
|
} else {
|
|
mjSNPRINTF(message, "%zu", nbytes >> (10*(6-i)));
|
|
}
|
|
return message;
|
|
}
|
|
|
|
|
|
// warning text
|
|
const char* mju_warningText(int warning, size_t info) {
|
|
static mjTHREADLOCAL char str[1000];
|
|
|
|
switch ((mjtWarning) warning) {
|
|
case mjWARN_INERTIA:
|
|
mjSNPRINTF(str, "Inertia matrix is too close to singular at DOF %zu. Check model.", info);
|
|
break;
|
|
|
|
case mjWARN_CONTACTFULL:
|
|
mjSNPRINTF(str,
|
|
"Too many contacts. The arena memory is full, increase arena memory allocation."
|
|
"(ncon = %zu)", info);
|
|
break;
|
|
|
|
case mjWARN_CNSTRFULL:
|
|
mjSNPRINTF(str,
|
|
"Insufficient arena memory for the number of constraints generated. "
|
|
"Increase arena memory allocation above %s bytes.", mju_writeNumBytes(info));
|
|
break;
|
|
|
|
case mjWARN_BADQPOS:
|
|
mjSNPRINTF(str, "Nan, Inf or huge value in QPOS at DOF %zu. The simulation is unstable.", info);
|
|
break;
|
|
|
|
case mjWARN_BADQVEL:
|
|
mjSNPRINTF(str, "Nan, Inf or huge value in QVEL at DOF %zu. The simulation is unstable.", info);
|
|
break;
|
|
|
|
case mjWARN_BADQACC:
|
|
mjSNPRINTF(str, "Nan, Inf or huge value in QACC at DOF %zu. The simulation is unstable.", info);
|
|
break;
|
|
|
|
case mjWARN_BADCTRL:
|
|
mjSNPRINTF(str, "Nan, Inf or huge value in CTRL at ACTUATOR %zu. The simulation is unstable.",
|
|
info);
|
|
break;
|
|
|
|
default:
|
|
mjSNPRINTF(str, "Unknown warning type %d.", warning);
|
|
}
|
|
|
|
return str;
|
|
}
|
|
|
|
|
|
// return 1 if nan or abs(x)>mjMAXVAL, 0 otherwise
|
|
int mju_isBad(mjtNum x) {
|
|
return (x != x || x > mjMAXVAL || x < -mjMAXVAL);
|
|
}
|
|
|
|
|
|
// return 1 if all elements are 0
|
|
int mju_isZero(const mjtNum* vec, int n) {
|
|
for (int i=0; i < n; i++) {
|
|
if (vec[i] != 0) {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
|
|
// return 1 if all elements are 0
|
|
int mju_isZeroByte(const unsigned char* vec, int n) {
|
|
if (!n || *vec) return !n;
|
|
return memcmp(vec, vec + 1, n - 1) == 0;
|
|
}
|
|
|
|
|
|
// set integer vector to 0
|
|
void mju_zeroInt(int* res, int n) {
|
|
memset(res, 0, n*sizeof(int));
|
|
}
|
|
|
|
|
|
// copy int vector vec into res
|
|
void mju_copyInt(int* res, const int* vec, int n) {
|
|
memcpy(res, vec, n*sizeof(int));
|
|
}
|
|
|
|
// fill int vector with val
|
|
void mju_fillInt(int* res, int val, int n) {
|
|
for (int i = 0; i < n; i++) {
|
|
res[i] = val;
|
|
}
|
|
}
|
|
|
|
// standard normal random number generator (optional second number)
|
|
mjtNum mju_standardNormal(mjtNum* num2) {
|
|
const mjtNum scale = 2.0/((mjtNum)RAND_MAX);
|
|
mjtNum x1, x2, w;
|
|
|
|
do {
|
|
x1 = scale * (mjtNum)rand() - 1.0;
|
|
x2 = scale * (mjtNum)rand() - 1.0;
|
|
w = x1 * x1 + x2 * x2;
|
|
} while (w >= 1.0 || w == 0);
|
|
|
|
w = mju_sqrt((-2.0 * mju_log(w)) / w);
|
|
if (num2) {
|
|
*num2 = x2 * w;
|
|
}
|
|
|
|
return (x1 * w);
|
|
}
|
|
|
|
|
|
// convert from float to mjtNum
|
|
void mju_f2n(mjtNum* res, const float* vec, int n) {
|
|
for (int i=0; i < n; i++) {
|
|
res[i] = (mjtNum) vec[i];
|
|
}
|
|
}
|
|
|
|
|
|
// convert from mjtNum to float
|
|
void mju_n2f(float* res, const mjtNum* vec, int n) {
|
|
for (int i=0; i < n; i++) {
|
|
res[i] = (float) vec[i];
|
|
}
|
|
}
|
|
|
|
|
|
// convert from double to mjtNum
|
|
void mju_d2n(mjtNum* res, const double* vec, int n) {
|
|
for (int i=0; i < n; i++) {
|
|
res[i] = (mjtNum) vec[i];
|
|
}
|
|
}
|
|
|
|
|
|
// convert from mjtNum to double
|
|
void mju_n2d(double* res, const mjtNum* vec, int n) {
|
|
for (int i=0; i < n; i++) {
|
|
res[i] = (double) vec[i];
|
|
}
|
|
}
|
|
|
|
|
|
// gather
|
|
void mju_gather(mjtNum* restrict res, const mjtNum* restrict vec, const int* restrict ind, int n) {
|
|
for (int i=0; i < n; i++) {
|
|
res[i] = vec[ind[i]];
|
|
}
|
|
}
|
|
|
|
|
|
// masked gather (set to 0 at negative indices)
|
|
void mju_gatherMasked(mjtNum* restrict res, const mjtNum* restrict vec,
|
|
const int* restrict ind, int n) {
|
|
for (int i=0; i < n; i++) {
|
|
res[i] = ind[i] >= 0 ? vec[ind[i]] : 0;
|
|
}
|
|
}
|
|
|
|
|
|
// scatter
|
|
void mju_scatter(mjtNum* restrict res, const mjtNum* restrict vec, const int* restrict ind, int n) {
|
|
for (int i=0; i < n; i++) {
|
|
res[ind[i]] = vec[i];
|
|
}
|
|
}
|
|
|
|
|
|
// gather integers
|
|
void mju_gatherInt(int* restrict res, const int* restrict vec, const int* restrict ind, int n) {
|
|
for (int i=0; i < n; i++) {
|
|
res[i] = vec[ind[i]];
|
|
}
|
|
}
|
|
|
|
|
|
// scatter integers
|
|
void mju_scatterInt(int* restrict res, const int* restrict vec, const int* restrict ind, int n) {
|
|
for (int i=0; i < n; i++) {
|
|
res[ind[i]] = vec[i];
|
|
}
|
|
}
|
|
|
|
|
|
// build gather indices mapping src to res, assumes pattern(res) \subseteq pattern(src)
|
|
void mju_sparseMap(int* map, int nr,
|
|
const int* res_rowadr, const int* res_rownnz, const int* res_colind,
|
|
const int* src_rowadr, const int* src_rownnz, const int* src_colind) {
|
|
for (int i = 0; i < nr; i++) {
|
|
int res_cursor = res_rowadr[i];
|
|
int res_end = res_cursor + res_rownnz[i];
|
|
int src_cursor = src_rowadr[i];
|
|
int src_end = src_cursor + src_rownnz[i];
|
|
|
|
while (res_cursor < res_end) {
|
|
int res_col = res_colind[res_cursor];
|
|
while (src_cursor < src_end && src_colind[src_cursor] < res_col) {
|
|
src_cursor++;
|
|
}
|
|
|
|
// found match, set index and advance cursors
|
|
map[res_cursor++] = src_cursor++;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// build masked-gather map to copy a lower-triangular src into symmetric res
|
|
// `cursor` is a preallocated buffer of size `nr`
|
|
void mju_lower2SymMap(int* map, int nr,
|
|
const int* res_rowadr, const int* res_rownnz, const int* res_colind,
|
|
const int* src_rowadr, const int* src_rownnz, const int* src_colind,
|
|
int* cursor) {
|
|
if (!nr) return;
|
|
|
|
// default all map entries to "no source"
|
|
int nnz = res_rowadr[nr-1] + res_rownnz[nr-1];
|
|
mju_fillInt(map, -1, nnz);
|
|
|
|
// initialize per-row cursor
|
|
for (int i = 0; i < nr; i++) {
|
|
cursor[i] = res_rowadr[i];
|
|
}
|
|
|
|
// sweep src rows; for each lower (i,j) set res(i,j) and res(j,i)
|
|
for (int i = 0; i < nr; i++) {
|
|
int src_start = src_rowadr[i];
|
|
int src_end = src_start + src_rownnz[i];
|
|
|
|
// sweep src row
|
|
for (int k = src_start; k < src_end; k++) {
|
|
int j = src_colind[k];
|
|
if (j > i) break; // use only lower triangle of src
|
|
|
|
// --- lower triangle: res(i, j)
|
|
int res_start = res_rowadr[i];
|
|
int res_end = res_start + res_rownnz[i];
|
|
int c = cursor[i];
|
|
|
|
// increment c until there is a match
|
|
while (c < res_end && res_colind[c] < j) c++;
|
|
|
|
// found match, set index, advance and save cursor
|
|
if (c < res_end && res_colind[c] == j) {
|
|
map[c] = k;
|
|
c++;
|
|
}
|
|
cursor[i] = c;
|
|
|
|
|
|
// --- upper mirror: res(j, i)
|
|
if (j != i) {
|
|
res_start = res_rowadr[j];
|
|
res_end = res_start + res_rownnz[j];
|
|
c = cursor[j];
|
|
|
|
// increment c until there is a match
|
|
while (c < res_end && res_colind[c] < i) c++;
|
|
|
|
// found match, set index and advance and save cursor
|
|
if (c < res_end && res_colind[c] == i) {
|
|
map[c] = k;
|
|
c++;
|
|
}
|
|
cursor[j] = c;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// insertion sort, increasing order
|
|
void mju_insertionSort(mjtNum* list, int n) {
|
|
for (int i=1; i < n; i++) {
|
|
mjtNum x = list[i];
|
|
int j = i-1;
|
|
while (j >= 0 && list[j] > x) {
|
|
list[j+1] = list[j];
|
|
j--;
|
|
}
|
|
list[j+1] = x;
|
|
}
|
|
}
|
|
|
|
|
|
// integer insertion sort, increasing order
|
|
void mju_insertionSortInt(int* list, int n) {
|
|
for (int i=1; i < n; i++) {
|
|
int x = list[i];
|
|
int j = i-1;
|
|
while (j >= 0 && list[j] > x) {
|
|
list[j+1] = list[j];
|
|
j--;
|
|
}
|
|
list[j+1] = x;
|
|
}
|
|
}
|
|
|
|
|
|
// Halton sequence
|
|
mjtNum mju_Halton(int index, int base) {
|
|
int n0 = index;
|
|
mjtNum b = (mjtNum)base;
|
|
mjtNum f = 1/b, hn = 0;
|
|
|
|
while (n0 > 0) {
|
|
int n1 = n0/base;
|
|
int r = n0 - n1*base;
|
|
hn += f*r;
|
|
f /= b;
|
|
n0 = n1;
|
|
}
|
|
|
|
return hn;
|
|
}
|
|
|
|
|
|
// Call strncpy, then set dst[n-1] = 0.
|
|
char* mju_strncpy(char *dst, const char *src, int n) {
|
|
if (dst && src && n > 0) {
|
|
strncpy(dst, src, n);
|
|
dst[n-1] = 0;
|
|
}
|
|
|
|
return dst;
|
|
}
|
|
|
|
|
|
// polynomial force coefficient: force = -x * mju_polyForce(...)
|
|
// flg_odd=0: linear + poly[0]*x + poly[1]*x^2 + ...
|
|
// flg_odd=1: linear + poly[0]*|x| + poly[1]*x^2 + ... (p is even, p*x is odd)
|
|
mjtNum mju_polyForce(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd) {
|
|
x = flg_odd ? mju_abs(x) : x;
|
|
mjtNum res = linear;
|
|
|
|
mjtNum xpow = 1;
|
|
for (int i=0; i < n; i++) {
|
|
xpow *= x;
|
|
res += poly[i] * xpow;
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// derivative of (x * mju_polyForce) w.r.t. x
|
|
mjtNum mjd_xPolyForce(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd) {
|
|
x = flg_odd ? mju_abs(x) : x;
|
|
mjtNum res = linear;
|
|
|
|
mjtNum xpow = 1;
|
|
for (int i=0; i < n; i++) {
|
|
xpow *= x;
|
|
res += (i+2) * poly[i] * xpow;
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// potential energy: integral from 0 to x of mju_polyForce(t) * t dt
|
|
mjtNum mju_polyPotential(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd) {
|
|
x = flg_odd ? mju_abs(x) : x;
|
|
mjtNum res = 0.5 * linear * (x * x);
|
|
|
|
mjtNum xpow = x;
|
|
for (int i=0; i < n; i++) {
|
|
xpow *= x;
|
|
res += poly[i] / (i+3) * (xpow * x);
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// sigmoid function over 0<=x<=1 using quintic polynomial
|
|
mjtNum mju_sigmoid(mjtNum x) {
|
|
// fast return
|
|
if (x <= 0) {
|
|
return 0;
|
|
}
|
|
if (x >= 1) {
|
|
return 1;
|
|
}
|
|
|
|
// sigmoid: f(x) = 6*x^5 - 15*x^4 + 10*x^3
|
|
// solution of f(0) = f'(0) = f''(0) = 0, f(1) = 1, f'(1) = f''(1) = 0
|
|
return x*x*x * (3*x * (2*x - 5) + 10);
|
|
}
|