#include "engine/engine_ray.h"

#include <math.h>

#include <stddef.h>

#include <mujoco/mjdata.h>

#include <mujoco/mjmacro.h>

#include <mujoco/mjmodel.h>

#include <mujoco/mjsan.h> // IWYU pragma: keep

#include <mujoco/mjvisualize.h>

#include "engine/engine_collision_sdf.h"

#include "engine/engine_memory.h"

#include "engine/engine_util_blas.h"

#include "engine/engine_util_errmem.h"

#include "engine/engine_util_misc.h"

#include "engine/engine_util_spatial.h"

static void ray_map(const mjtNum pos[3], const mjtNum mat[9],

const mjtNum pnt[3], const mjtNum vec[3],

mjtNum lpnt[3], mjtNum lvec[3]) {

const mjtNum dif[3] = {pnt[0]-pos[0], pnt[1]-pos[1], pnt[2]-pos[2]};

// lpnt = mat' * dif

lpnt[0] = mat[0]*dif[0] + mat[3]*dif[1] + mat[6]*dif[2];

lpnt[1] = mat[1]*dif[0] + mat[4]*dif[1] + mat[7]*dif[2];

lpnt[2] = mat[2]*dif[0] + mat[5]*dif[1] + mat[8]*dif[2];

// lvec = mat' * vec

lvec[0] = mat[0]*vec[0] + mat[3]*vec[1] + mat[6]*vec[2];

lvec[1] = mat[1]*vec[0] + mat[4]*vec[1] + mat[7]*vec[2];

lvec[2] = mat[2]*vec[0] + mat[5]*vec[1] + mat[8]*vec[2];

}

static mjtNum longitude(const mjtNum vec[3]) {

return mju_atan2(vec[1], vec[0]);

}

static mjtNum latitude(const mjtNum vec[3]) {

return mju_atan2(mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1]), vec[2]);

}

static int ray_eliminate(const mjModel* m, const mjData* d, int geomid,

const mjtByte* geomgroup, mjtBool flg_static, int bodyexclude) {

// body exclusion

if (m->geom_bodyid[geomid] == bodyexclude) {

return 1;

}

// invisible geom exclusion

if (m->geom_matid[geomid] < 0 && m->geom_rgba[4*geomid+3] == 0) {

return 1;

}

// invisible material exclusion

if (m->geom_matid[geomid] >= 0 && m->mat_rgba[4*m->geom_matid[geomid]+3] == 0) {

return 1;

}

// static exclusion

if (!flg_static && m->body_weldid[m->geom_bodyid[geomid]] == 0) {

return 1;

}

// no geomgroup inclusion

if (!geomgroup) {

return 0;

}

// group inclusion/exclusion

int groupid = mjMIN(mjNGROUP-1, mjMAX(0, m->geom_group[geomid]));

return (geomgroup[groupid] == 0);

}

static mjtNum ray_quad(mjtNum a, mjtNum b, mjtNum c, mjtNum x[2]) {

// compute determinant

mjtNum det = b*b - a*c;

// return if real finite solutions don't exist

if (det < 0 || a < mjMINVAL) {

x[0] = -1;

x[1] = -1;

return -1;

}

// compute the two solutions, x[0] <= x[1] is guaranteed

det = mju_sqrt(det);

x[0] = (-b-det)/a;

x[1] = (-b+det)/a;

// return smallest non-negative solution

if (x[0] >= 0) {

return x[0];

} else if (x[1] >= 0) {

return x[1];

}

// both solutions are negative

return -1;

}

mjtNum ray_triangle(mjtNum v[][3], const mjtNum lpnt[3], const mjtNum lvec[3],

const mjtNum b0[3], const mjtNum b1[3], mjtNum normal[3]) {

// clear normal if given

if (normal) mju_zero3(normal);

// dif = v[i] - lpnt

mjtNum dif[3][3];

for (int i=0; i < 3; i++) {

for (int j=0; j < 3; j++) {

dif[i][j] = v[i][j] - lpnt[j];

}

}

// project difference vectors in normal plane

mjtNum planar[3][2];

for (int i=0; i < 3; i++) {

planar[i][0] = mju_dot3(b0, dif[i]);

planar[i][1] = mju_dot3(b1, dif[i]);

}

// reject if on the same side of any coordinate axis

if ((planar[0][0] > 0 && planar[1][0] > 0 && planar[2][0] > 0) ||

(planar[0][0] < 0 && planar[1][0] < 0 && planar[2][0] < 0) ||

(planar[0][1] > 0 && planar[1][1] > 0 && planar[2][1] > 0) ||

(planar[0][1] < 0 && planar[1][1] < 0 && planar[2][1] < 0)) {

return -1;

}

// determine if origin is inside planar projection of triangle

// A = (p0-p2, p1-p2), b = -p2, solve A*t = b

mjtNum A[4] = {planar[0][0]-planar[2][0], planar[1][0]-planar[2][0],

planar[0][1]-planar[2][1], planar[1][1]-planar[2][1]};

mjtNum b[2] = {-planar[2][0], -planar[2][1]};

mjtNum det = A[0]*A[3] - A[1]*A[2];

if (mju_abs(det) < mjMINVAL) {

return -1;

}

mjtNum t0 = ( A[3]*b[0] - A[1]*b[1]) / det;

mjtNum t1 = (-A[2]*b[0] + A[0]*b[1]) / det;

// check if outside

if (t0 < 0 || t1 < 0|| t0+t1 > 1) {

return -1;

}

// intersect ray with plane of triangle

mju_sub3(dif[0], v[0], v[2]); // v0-v2

mju_sub3(dif[1], v[1], v[2]); // v1-v2

mju_sub3(dif[2], lpnt, v[2]); // lp-v2

mjtNum nrm[3];

mju_cross(nrm, dif[0], dif[1]); // normal to triangle plane

mjtNum denom = mju_dot3(lvec, nrm);

if (mju_abs(denom) < mjMINVAL) {

return -1;

}

// compute distance

mjtNum x = -mju_dot3(dif[2], nrm) / denom;

// compute normal if given

if (normal) {

mju_normalize3(nrm);

mju_copy3(normal, nrm);

}

return x;

}

static mjtNum ray_plane(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3],

const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) {

// clear normal if given

if (normal) mju_zero3(normal);

// map to local frame

mjtNum lpnt[3], lvec[3];

ray_map(pos, mat, pnt, vec, lpnt, lvec);

// z-vec not pointing towards front face: reject

if (lvec[2] > -mjMINVAL) {

return -1;

}

// intersection with plane

const mjtNum x = -lpnt[2]/lvec[2];

if (x < 0) {

return -1;

}

mjtNum p0 = lpnt[0] + x*lvec[0];

mjtNum p1 = lpnt[1] + x*lvec[1];

// accept only within rendered rectangle

if ((size[0] <= 0 || mju_abs(p0) <= size[0]) &&

(size[1] <= 0 || mju_abs(p1) <= size[1])) {

if (normal) {

normal[0] = mat[2];

normal[1] = mat[5];

normal[2] = mat[8];

}

return x;

} else {

return -1;

}

}

static mjtNum ray_sphere(const mjtNum pos[3], const mjtNum mat[9], mjtNum dist_sqr,

const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) {

// (x*vec+pnt-pos)'*(x*vec+pnt-pos) = size[0]*size[0]

mjtNum dif[3] = {pnt[0]-pos[0], pnt[1]-pos[1], pnt[2]-pos[2]};

mjtNum a = vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2];

mjtNum b = vec[0]*dif[0] + vec[1]*dif[1] + vec[2]*dif[2];

mjtNum c = dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2] - dist_sqr;

// solve a*x^2 + 2*b*x + c = 0

mjtNum xx[2];

mjtNum x = ray_quad(a, b, c, xx);

// compute normal if required

if (normal) {

if (x < 0) {

mju_zero3(normal);

} else {

// normal at surface intersection s (global frame)

mjtNum s[3];

mju_addScl3(s, pnt, vec, x);

mju_sub3(normal, s, pos);

mju_normalize3(normal);

}

}

return x;

}

static mjtNum ray_capsule(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3],

const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) {

// bounding sphere test

mjtNum ssz = size[0] + size[1];

if (ray_sphere(pos, NULL, ssz * ssz, pnt, vec, NULL) < 0) {

if (normal) mju_zero3(normal);

return -1;

}

// map to local frame

mjtNum lpnt[3], lvec[3];

ray_map(pos, mat, pnt, vec, lpnt, lvec);

// init solution

mjtNum x = -1, sol, xx[2];

int type; // -1: bottom, 0: cylinder, 1: top

// cylinder round side: (x*lvec+lpnt)'*(x*lvec+lpnt) = size[0]*size[0]

mjtNum a = lvec[0]*lvec[0] + lvec[1]*lvec[1];

mjtNum b = lvec[0]*lpnt[0] + lvec[1]*lpnt[1];

mjtNum c = lpnt[0]*lpnt[0] + lpnt[1]*lpnt[1] - size[0]*size[0];

// solve a*x^2 + 2*b*x + c = 0

sol = ray_quad(a, b, c, xx);

// make sure round solution is between flat sides

if (sol >= 0 && mju_abs(lpnt[2]+sol*lvec[2]) <= size[1]) {

if (x < 0 || sol < x) {

x = sol;

type = 0;

}

}

// top cap

mjtNum ldif[3] = {lpnt[0], lpnt[1], lpnt[2]-size[1]};

a = lvec[0]*lvec[0] + lvec[1]*lvec[1] + lvec[2]*lvec[2];

b = lvec[0]*ldif[0] + lvec[1]*ldif[1] + lvec[2]*ldif[2];

c = ldif[0]*ldif[0] + ldif[1]*ldif[1] + ldif[2]*ldif[2] - size[0]*size[0];

ray_quad(a, b, c, xx);

// accept only top half of sphere

for (int i=0; i < 2; i++) {

if (xx[i] >= 0 && lpnt[2]+xx[i]*lvec[2] >= size[1]) {

if (x < 0 || xx[i] < x) {

x = xx[i];

type = 1;

}

}

}

// bottom cap

ldif[2] = lpnt[2]+size[1];

b = lvec[0]*ldif[0] + lvec[1]*ldif[1] + lvec[2]*ldif[2];

c = ldif[0]*ldif[0] + ldif[1]*ldif[1] + ldif[2]*ldif[2] - size[0]*size[0];

ray_quad(a, b, c, xx);

// accept only bottom half of sphere

for (int i=0; i < 2; i++) {

if (xx[i] >= 0 && lpnt[2]+xx[i]*lvec[2] <= -size[1]) {

if (x < 0 || xx[i] < x) {

x = xx[i];

type = -1;

}

}

}

// compute normal if required

if (normal) {

if (x < 0) {

mju_zero3(normal);

} else {

normal[0] = lpnt[0] + lvec[0] * x;

normal[1] = lpnt[1] + lvec[1] * x;

normal[2] = (type == 0) ? 0 : lpnt[2] + lvec[2] * x - size[1] * type;

// normalize, rotate into global frame

mju_normalize3(normal);

mju_mulMatVec3(normal, mat, normal);

}

}

return x;

}

static mjtNum ray_ellipsoid(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3],

const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) {

// map to local frame

mjtNum lpnt[3], lvec[3];

ray_map(pos, mat, pnt, vec, lpnt, lvec);

// invert size^2

mjtNum s[3] = {1/(size[0]*size[0]), 1/(size[1]*size[1]), 1/(size[2]*size[2])};

// (x*lvec+lpnt)' * diag(1./size^2) * (x*lvec+lpnt) = 1

mjtNum a = s[0]*lvec[0]*lvec[0] + s[1]*lvec[1]*lvec[1] + s[2]*lvec[2]*lvec[2];

mjtNum b = s[0]*lvec[0]*lpnt[0] + s[1]*lvec[1]*lpnt[1] + s[2]*lvec[2]*lpnt[2];

mjtNum c = s[0]*lpnt[0]*lpnt[0] + s[1]*lpnt[1]*lpnt[1] + s[2]*lpnt[2]*lpnt[2] - 1;

// solve a*x^2 + 2*b*x + c = 0

mjtNum xx[2];

mjtNum x = ray_quad(a, b, c, xx);

// compute normal if required

if (normal) {

if (x < 0) {

mju_zero3(normal);

} else {

// surface intersection (local frame)

mjtNum l[3];

mju_addScl3(l, lpnt, lvec, x);

// gradient of ellipsoid function

normal[0] = s[0] * l[0];

normal[1] = s[1] * l[1];

normal[2] = s[2] * l[2];

// normalize, rotate into global frame

mju_normalize3(normal);

mju_mulMatVec3(normal, mat, normal);

}

}

return x;

}

static mjtNum ray_cylinder(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3],

const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) {

// bounding sphere test

mjtNum ssz = size[0]*size[0] + size[1]*size[1];

if (ray_sphere(pos, NULL, ssz, pnt, vec, NULL) < 0) {

if (normal) mju_zero3(normal);

return -1;

}

// map to local frame

mjtNum lpnt[3], lvec[3];

ray_map(pos, mat, pnt, vec, lpnt, lvec);

// init solution

mjtNum x = -1, sol;

int type = 0; // -1: bottom, 0: round, 1: top

// flat sides

int side;

if (mju_abs(lvec[2]) > mjMINVAL) {

for (side=-1; side <= 1; side+=2) {

// solution of: lpnt[2] + x*lvec[2] = side*height_size

sol = (side*size[1]-lpnt[2])/lvec[2];

// process if non-negative

if (sol >= 0) {

// intersection with horizontal face

mjtNum p0 = lpnt[0] + sol*lvec[0];

mjtNum p1 = lpnt[1] + sol*lvec[1];

// accept within radius

if (p0*p0 + p1*p1 <= size[0]*size[0]) {

if (x < 0 || sol < x) {

x = sol;

type = side;

}

}

}

}

}

// round side: (x*lvec+lpnt)'*(x*lvec+lpnt) = size[0]*size[0]

mjtNum a = lvec[0]*lvec[0] + lvec[1]*lvec[1];

mjtNum b = lvec[0]*lpnt[0] + lvec[1]*lpnt[1];

mjtNum c = lpnt[0]*lpnt[0] + lpnt[1]*lpnt[1] - size[0]*size[0];

// solve a*x^2 + 2*b*x + c = 0

mjtNum xx[2];

sol = ray_quad(a, b, c, xx);

// make sure round solution is between flat sides

if (sol >= 0 && mju_abs(lpnt[2]+sol*lvec[2]) <= size[1]) {

if (x < 0 || sol < x) {

x = sol;

type = 0;

}

}

// compute normal if required

if (normal) {

if (x < 0) {

mju_zero3(normal);

} else {

// round side

if (type == 0) {

// normal at surface intersection (local frame)

normal[0] = lpnt[0] + lvec[0] * x;

normal[1] = lpnt[1] + lvec[1] * x;

normal[2] = 0;

mju_normalize3(normal);

}

// flat sides

else {

normal[0] = 0;

normal[1] = 0;

normal[2] = type;

}

// rotate into global frame

mju_mulMatVec3(normal, mat, normal);

}

}

return x;

}

static mjtNum ray_box(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3],

const mjtNum pnt[3], const mjtNum vec[3], mjtNum all[6], mjtNum normal[3]) {

// clear outputs

if (all) all[0] = all[1] = all[2] = all[3] = all[4] = all[5] = -1;

if (normal) mju_zero3(normal);

// bounding sphere test

mjtNum ssz = size[0]*size[0] + size[1]*size[1] + size[2]*size[2];

if (ray_sphere(pos, NULL, ssz, pnt, vec, NULL) < 0) {

return -1;

}

// faces

const int iface[3][2] = {

{1, 2},

{0, 2},

{0, 1}

};

// map to local frame

mjtNum lpnt[3], lvec[3];

ray_map(pos, mat, pnt, vec, lpnt, lvec);

// init solution

mjtNum x = -1, sol;

int face_side, face_axis = -1;

// loop over axes with non-zero vec

for (int i=0; i < 3; i++) {

if (mju_abs(lvec[i]) > mjMINVAL) {

for (int side=-1; side <= 1; side+=2) {

// solution of: lpnt[i] + x*lvec[i] = side*size[i]

sol = (side*size[i]-lpnt[i])/lvec[i];

// process if non-negative

if (sol >= 0) {

// intersection with face

mjtNum p0 = lpnt[iface[i][0]] + sol*lvec[iface[i][0]];

mjtNum p1 = lpnt[iface[i][1]] + sol*lvec[iface[i][1]];

// accept within rectangle

if (mju_abs(p0) <= size[iface[i][0]] &&

mju_abs(p1) <= size[iface[i][1]]) {

// update

if (x < 0 || sol < x) {

x = sol;

face_axis = i;

face_side = side;

}

// save in all

if (all) {

all[2*i+(side+1)/2] = sol;

}

}

}

}

}

}

// compute normal if required

if (normal && x >= 0) {

mjtNum n_local[3] = {0, 0, 0};

n_local[face_axis] = face_side;

mju_mulMatVec3(normal, mat, n_local);

}

return x;

}

mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int geomid,

const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) {

// clear normal if given

if (normal) mju_zero3(normal);

// check geom type

if (m->geom_type[geomid] != mjGEOM_HFIELD) {

mjERROR("geom with hfield type expected");

}

// hfield id and dimensions

int hid = m->geom_dataid[geomid];

int nrow = m->hfield_nrow[hid];

int ncol = m->hfield_ncol[hid];

const mjtNum* size = m->hfield_size + 4*hid;

const float* data = m->hfield_data + m->hfield_adr[hid];

// compute size and pos of base box

mjtNum base_size[3] = {size[0], size[1], size[3]*0.5};

const mjtNum* xmat = d->geom_xmat + 9*geomid;

const mjtNum* xpos = d->geom_xpos + 3*geomid;

mjtNum base_pos[3] = {

xpos[0] - xmat[2]*size[3]*0.5,

xpos[1] - xmat[5]*size[3]*0.5,

xpos[2] - xmat[8]*size[3]*0.5

};

// compute size and pos of top box

mjtNum top_size[3] = {size[0], size[1], size[2]*0.5};

mjtNum top_pos[3] = {

xpos[0] + xmat[2]*size[2]*0.5,

xpos[1] + xmat[5]*size[2]*0.5,

xpos[2] + xmat[8]*size[2]*0.5

};

// init: intersection with base box

mjtNum normal_base[3];

mjtNum x = ray_box(base_pos, xmat, base_size, pnt, vec,

NULL, normal ? normal_base : NULL);

// check top box: done if no intersection

mjtNum all[6];

mjtNum top_intersect = ray_box(top_pos, xmat, top_size, pnt, vec, all, NULL);

if (top_intersect < 0) {

if (normal && x >= 0) mju_copy3(normal, normal_base);

return x;

}

// map to local frame

mjtNum lpnt[3], lvec[3];

ray_map(xpos, xmat, pnt, vec, lpnt, lvec);

// construct basis vectors of normal plane

mjtNum b0[3] = {1, 1, 1}, b1[3];

if (mju_abs(lvec[0]) >= mju_abs(lvec[1]) &&

mju_abs(lvec[0]) >= mju_abs(lvec[2])) {

b0[0] = 0;

} else if (mju_abs(lvec[1]) >= mju_abs(lvec[2])) {

b0[1] = 0;

} else {

b0[2] = 0;

}

mju_addScl3(b1, b0, lvec, -mju_dot3(lvec, b0)/mju_dot3(lvec, lvec));

mju_normalize3(b1);

mju_cross(b0, b1, lvec);

mju_normalize3(b0);

// find ray segment intersecting top box

mjtNum seg[2] = {0, top_intersect};

for (int i=0; i < 6; i++) {

if (all[i] > seg[1]) {

seg[0] = top_intersect;

seg[1] = all[i];

}

}

// project segment endpoints in horizontal plane, discretize

mjtNum dx = (2.0*size[0]) / (ncol-1);

mjtNum dy = (2.0*size[1]) / (nrow-1);

mjtNum SX[2], SY[2];

for (int i=0; i < 2; i++) {

SX[i] = (lpnt[0] + seg[i]*lvec[0] + size[0]) / dx;

SY[i] = (lpnt[1] + seg[i]*lvec[1] + size[1]) / dy;

}

// compute ranges, with +1 padding

int cmin = mjMAX(0, (int)mju_floor(mjMIN(SX[0], SX[1]))-1);

int cmax = mjMIN(ncol-1, (int)mju_ceil(mjMAX(SX[0], SX[1]))+1);

int rmin = mjMAX(0, (int)mju_floor(mjMIN(SY[0], SY[1]))-1);

int rmax = mjMIN(nrow-1, (int)mju_ceil(mjMAX(SY[0], SY[1]))+1);

// local normal, initialize with base box normal (if any), in local frame

mjtNum normal_local[3] = {0, 0, 0};

if (normal && x >= 0) {

mju_mulMatTVec3(normal_local, xmat, normal_base);

}

// check triangles within bounds

for (int r=rmin; r < rmax; r++) {

for (int c=cmin; c < cmax; c++) {

// triangle normal

mjtNum normal_tri[3];

// first triangle: swap v1 and v2 for consistent CCW winding (normals point up)

mjtNum va[3][3] = {

{dx*c-size[0], dy*r-size[1], data[r*ncol+c]*size[2]},

{dx*(c+1)-size[0], dy*(r+0)-size[1], data[(r+0)*ncol+(c+1)]*size[2]},

{dx*(c+1)-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+(c+1)]*size[2]}

};

mjtNum sol = ray_triangle(va, lpnt, lvec, b0, b1, normal ? normal_tri : NULL);

if (sol >= 0 && (x < 0 || sol < x)) {

x = sol;

if (normal) mju_copy3(normal_local, normal_tri);

}

// second triangle

mjtNum vb[3][3] = {

{dx*c-size[0], dy*r-size[1], data[r*ncol+c]*size[2]},

{dx*(c+1)-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+(c+1)]*size[2]},

{dx*(c+0)-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+(c+0)]*size[2]}

};

sol = ray_triangle(vb, lpnt, lvec, b0, b1, normal ? normal_tri : NULL);

if (sol >= 0 && (x < 0 || sol < x)) {

x = sol;

if (normal) mju_copy3(normal_local, normal_tri);

}

}

}

// check viable sides of top box

for (int i=0; i < 4; i++) {

if (all[i] >= 0 && (all[i] < x || x < 0)) {

// normalized height of intersection point

mjtNum z = (lpnt[2] + all[i]*lvec[2]) / size[2];

// rectangle points

mjtNum y, y0, z0, z1;

// side normal to x-axis

if (i < 2) {

y = (lpnt[1] + all[i]*lvec[1] + size[1]) / dy;

y0 = mjMAX(0, mjMIN(nrow-2, mju_floor(y)));

z0 = (mjtNum)data[mju_round(y0+0)*ncol + (i == 1 ? ncol-1 : 0)];

z1 = (mjtNum)data[mju_round(y0+1)*ncol + (i == 1 ? ncol-1 : 0)];

}

// side normal to y-axis

else {

y = (lpnt[0] + all[i]*lvec[0] + size[0]) / dx;

y0 = mjMAX(0, mjMIN(ncol-2, mju_floor(y)));

z0 = (mjtNum)data[mju_round(y0+0) + (i == 3 ? (nrow-1)*ncol : 0)];

z1 = (mjtNum)data[mju_round(y0+1) + (i == 3 ? (nrow-1)*ncol : 0)];

}

// check if point is below line segment

if (z < z0*(y0+1-y) + z1*(y-y0)) {

x = all[i];

// compute normal

if (normal) {

mju_zero3(normal_local);

if (i == 0) normal_local[0] = -1;

else if (i == 1) normal_local[0] = 1;

else if (i == 2) normal_local[1] = -1;

else if (i == 3) normal_local[1] = 1;

}

}

}

}

// rotate normal to global frame

if (normal && x >= 0) {

mju_mulMatVec3(normal, xmat, normal_local);

}

return x;

}

int mju_raySlab(const mjtNum aabb[6], const mjtNum xpos[3],

const mjtNum xmat[9], const mjtNum pnt[3], const mjtNum vec[3]) {

mjtNum tmin = 0.0, tmax = INFINITY;

// compute min and max

mjtNum min[3] = {aabb[0]-aabb[3], aabb[1]-aabb[4], aabb[2]-aabb[5]};

mjtNum max[3] = {aabb[0]+aabb[3], aabb[1]+aabb[4], aabb[2]+aabb[5]};

// compute ray in local coordinates

mjtNum src[3], dir[3];

ray_map(xpos, xmat, pnt, vec, src, dir);

// check intersections

mjtNum invdir[3] = { 1.0 / dir[0], 1.0 / dir[1], 1.0 / dir[2] };

for (int d = 0; d < 3; ++d) {

mjtNum t1 = (min[d] - src[d]) * invdir[d];

mjtNum t2 = (max[d] - src[d]) * invdir[d];

mjtNum minval = t1 < t2 ? t1 : t2;

mjtNum maxval = t1 < t2 ? t2 : t1;

tmin = tmin > minval ? tmin : minval;

tmax = tmax < maxval ? tmax : maxval;

}

return tmin < tmax;

}

mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3],

const mjtNum vec[3], mjtNum normal[3]) {

// clear normal if given

if (normal) mju_zero3(normal);

int mark_active = m->vis.global.bvactive;

const int meshid = m->geom_dataid[id];

const int bvhadr = m->mesh_bvhadr[meshid];

const int* faceid = m->bvh_nodeid + bvhadr;

const mjtNum* bvh = m->bvh_aabb + 6*bvhadr;

const int* child = m->bvh_child + 2*bvhadr;

if (meshid == -1) {

mjERROR("mesh id of geom %d is -1", meshid); // SHOULD NOT OCCUR

}

// initialize stack

int stack[mjMAXTREEDEPTH];

int nstack = 0;

stack[nstack] = 0;

nstack++;

// map to local frame

mjtNum lpnt[3], lvec[3];

ray_map(d->geom_xpos+3*id, d->geom_xmat+9*id, pnt, vec, lpnt, lvec);

// construct basis vectors of normal plane

mjtNum b0[3] = {1, 1, 1}, b1[3];

if (mju_abs(lvec[0]) >= mju_abs(lvec[1]) && mju_abs(lvec[0]) >= mju_abs(lvec[2])) {

b0[0] = 0;

} else if (mju_abs(lvec[1]) >= mju_abs(lvec[2])) {

b0[1] = 0;

} else {

b0[2] = 0;

}

mju_addScl3(b1, b0, lvec, -mju_dot3(lvec, b0)/mju_dot3(lvec, lvec));

mju_normalize3(b1);

mju_cross(b0, b1, lvec);

mju_normalize3(b0);

// init solution

mjtNum x = -1, sol;

mjtNum normal_local[3];

while (nstack) {

// pop from stack

nstack--;

int node = stack[nstack];

// intersection test

int intersect = mju_raySlab(bvh+6*node, d->geom_xpos+3*id, d->geom_xmat+9*id, pnt, vec);

// if no intersection, skip

if (!intersect) {

continue;

}

// node1 is a leaf

if (faceid[node] != -1) {

int face = faceid[node] + m->mesh_faceadr[meshid];

// get float vertices

float* vf[3];

vf[0] = m->mesh_vert + 3*(m->mesh_face[3*face+0] + m->mesh_vertadr[meshid]);

vf[1] = m->mesh_vert + 3*(m->mesh_face[3*face+1] + m->mesh_vertadr[meshid]);

vf[2] = m->mesh_vert + 3*(m->mesh_face[3*face+2] + m->mesh_vertadr[meshid]);

// convert to mjtNum

mjtNum v[3][3];

for (int i=0; i < 3; i++) {

for (int j=0; j < 3; j++) {

v[i][j] = (mjtNum)vf[i][j];

}

}

// solve

sol = ray_triangle(v, lpnt, lvec, b0, b1, normal ? normal_local : NULL);

// update

if (sol >= 0 && (x < 0 || sol < x)) {

x = sol;

if (normal) mju_copy3(normal, normal_local);

if (mark_active) d->bvh_active[node + bvhadr] = true;

}

continue;

}

// used for rendering

if (mark_active) {

d->bvh_active[node + bvhadr] = true;

}

// add children to the stack

for (int i=0; i < 2; i++) {

if (child[2*node+i] != -1) {

if (nstack >= mjMAXTREEDEPTH) {

mjERROR("BVH stack depth exceeded in geom %d.", id);

}

stack[nstack] = child[2*node+i];

nstack++;

}

}

}

// rotate normal to global frame

if (normal && x >= 0) {

mju_mulMatVec3(normal, d->geom_xmat+9*id, normal);

}

return x;

}

static mjtNum mj_raySdf(const mjModel* m, const mjData* d, int g,

const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) {

if (normal) mju_zero3(normal);

mjtNum distance_total = 0;

mjtNum p[3];

mjtNum kMinDist = 1e-7;

// exclude using bounding box

if (ray_box(d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, pnt, vec, NULL, NULL) < 0) {

return -1;

}

// get sdf plugin

int instance = m->geom_plugin[g];