double ChCollisionUtils::PointLineDistance(Vector p, Vector dA, Vector dB, double& mu, int& is_insegment) {
mu = -1.0;
is_insegment = 0;
double mdist = 10e34;
Vector vseg = Vsub(dB, dA);
Vector vdir = Vnorm(vseg);
Vector vray = Vsub(p, dA);
mdist = Vlength(Vcross(vray, vdir));
mu = Vdot(vray, vdir) / Vlength(vseg);
if ((mu >= 0) && (mu <= 1.0))
is_insegment = 1;
return mdist;
}
inline
void
compute_moment(moment &M, PQP_REAL p[3], PQP_REAL q[3], PQP_REAL r[3])
{
PQP_REAL u[3], v[3], w[3];
// compute the area of the triangle
VmV(u, q, p);
VmV(v, r, p);
VcrossV(w, u, v);
M.A = 0.5 * Vlength(w);
if (M.A == 0.0)
{
// This triangle has zero area. The second order components
// would be eliminated with the usual formula, so, for the
// sake of robustness we use an alternative form. These are the
// centroid and second-order components of the triangle's vertices.
// centroid
M.m[0] = (p[0] + q[0] + r[0]) /3;
M.m[1] = (p[1] + q[1] + r[1]) /3;
M.m[2] = (p[2] + q[2] + r[2]) /3;
// second-order components
M.s[0][0] = (p[0]*p[0] + q[0]*q[0] + r[0]*r[0]);
M.s[0][1] = (p[0]*p[1] + q[0]*q[1] + r[0]*r[1]);
M.s[0][2] = (p[0]*p[2] + q[0]*q[2] + r[0]*r[2]);
M.s[1][1] = (p[1]*p[1] + q[1]*q[1] + r[1]*r[1]);
M.s[1][2] = (p[1]*p[2] + q[1]*q[2] + r[1]*r[2]);
M.s[2][2] = (p[2]*p[2] + q[2]*q[2] + r[2]*r[2]);
M.s[2][1] = M.s[1][2];
M.s[1][0] = M.s[0][1];
M.s[2][0] = M.s[0][2];
return;
}
// get the centroid
M.m[0] = (p[0] + q[0] + r[0])/3;
M.m[1] = (p[1] + q[1] + r[1])/3;
M.m[2] = (p[2] + q[2] + r[2])/3;
// get the second order components -- note the weighting by the area
M.s[0][0] = M.A*(9*M.m[0]*M.m[0]+p[0]*p[0]+q[0]*q[0]+r[0]*r[0])/12;
M.s[0][1] = M.A*(9*M.m[0]*M.m[1]+p[0]*p[1]+q[0]*q[1]+r[0]*r[1])/12;
M.s[1][1] = M.A*(9*M.m[1]*M.m[1]+p[1]*p[1]+q[1]*q[1]+r[1]*r[1])/12;
M.s[0][2] = M.A*(9*M.m[0]*M.m[2]+p[0]*p[2]+q[0]*q[2]+r[0]*r[2])/12;
M.s[1][2] = M.A*(9*M.m[1]*M.m[2]+p[1]*p[2]+q[1]*q[2]+r[1]*r[2])/12;
M.s[2][2] = M.A*(9*M.m[2]*M.m[2]+p[2]*p[2]+q[2]*q[2]+r[2]*r[2])/12;
M.s[2][1] = M.s[1][2];
M.s[1][0] = M.s[0][1];
M.s[2][0] = M.s[0][2];
}
REAL computeDistance( const REAL * bv1_m_center, const REAL * bv2_m_center, const REAL bv1_m_rad, const REAL bv2_m_rad, REAL * rot, const REAL * trans)
{
#pragma HLS INLINE recursive
REAL T[3];
REAL Ttemp[3];
VmV(Ttemp,trans,bv1_m_center);
MxVpV(T, rot, bv2_m_center, Ttemp);
return 1.0;
return Vlength(T) - (bv1_m_rad + bv2_m_rad);
}
double ChCollisionUtils::PointTriangleDistance(Vector B,
Vector A1,
Vector A2,
Vector A3,
double& mu,
double& mv,
int& is_into,
Vector& Bprojected) {
// defaults
is_into = 0;
mu = mv = -1;
double mdistance = 10e22;
Vector Dx, Dy, Dz, T1, T1p;
Dx = Vsub(A2, A1);
Dz = Vsub(A3, A1);
Dy = Vcross(Dz, Dx);
double dylen = Vlength(Dy);
if (fabs(dylen) < EPS_TRIDEGEN) // degenerate triangle
return mdistance;
Dy = Vmul(Dy, 1.0 / dylen);
ChMatrix33<> mA;
ChMatrix33<> mAi;
mA.Set_A_axis(Dx, Dy, Dz);
// invert triangle coordinate matrix -if singular matrix, was degenerate triangle-.
if (fabs(mA.FastInvert(mAi)) < 0.000001)
return mdistance;
T1 = mAi.Matr_x_Vect(Vsub(B, A1));
T1p = T1;
T1p.y() = 0;
mu = T1.x();
mv = T1.z();
mdistance = -T1.y();
if (mu >= 0 && mv >= 0 && mv <= 1.0 - mu) {
is_into = 1;
Bprojected = Vadd(A1, mA.Matr_x_Vect(T1p));
}
return mdistance;
}
void Viewer::computeExtrusion(int nOrientation,
float *orientation,
int nScale,
float *scale,
int nCrossSection,
float *crossSection,
int nSpine,
float *spine,
float *c, // OUT: coordinates
float *tc, // OUT: texture coords
int *faces) // OUT: face list
{
int i, j, ci;
// Xscp, Yscp, Zscp- columns of xform matrix to align cross section
// with spine segments.
float Xscp[3] = { 1.0, 0.0, 0.0};
float Yscp[3] = { 0.0, 1.0, 0.0};
float Zscp[3] = { 0.0, 0.0, 1.0};
float lastZ[3];
// Is the spine a closed curve (last pt == first pt)?
bool spineClosed = (FPZERO(spine[ 3*(nSpine-1)+0 ] - spine[0]) &&
FPZERO(spine[ 3*(nSpine-1)+1 ] - spine[1]) &&
FPZERO(spine[ 3*(nSpine-1)+2 ] - spine[2]));
// Is the spine a straight line?
bool spineStraight = true;
for (i = 1; i < nSpine-1; ++i)
{
float v1[3], v2[3];
v1[0] = spine[3*(i-1)+0] - spine[3*(i)+0];
v1[1] = spine[3*(i-1)+1] - spine[3*(i)+1];
v1[2] = spine[3*(i-1)+2] - spine[3*(i)+2];
v2[0] = spine[3*(i+1)+0] - spine[3*(i)+0];
v2[1] = spine[3*(i+1)+1] - spine[3*(i)+1];
v2[2] = spine[3*(i+1)+2] - spine[3*(i)+2];
Vcross(v1, v2, v1);
if (Vlength(v1) != 0.0)
{
spineStraight = false;
Vnorm( v1 );
Vset( lastZ, v1 );
break;
}
}
// If the spine is a straight line, compute a constant SCP xform
if (spineStraight)
{
float V1[3] = { 0.0, 1.0, 0.0}, V2[3], V3[3];
V2[0] = spine[3*(nSpine-1)+0] - spine[0];
V2[1] = spine[3*(nSpine-1)+1] - spine[1];
V2[2] = spine[3*(nSpine-1)+2] - spine[2];
Vcross( V3, V2, V1 );
float len = (float)Vlength(V3);
if (len != 0.0) // Not aligned with Y axis
{
Vscale(V3, 1.0f/len);
float orient[4]; // Axis/angle
Vset(orient, V3);
orient[3] = acos(Vdot(V1,V2));
double scp[4][4]; // xform matrix
Mrotation( scp, orient );
for (int k=0; k<3; ++k) {
Xscp[k] = (float)scp[0][k];
Yscp[k] = (float)scp[1][k];
Zscp[k] = (float)scp[2][k];
}
}
}
// Orientation matrix
double om[4][4];
if (nOrientation == 1 && ! FPZERO(orientation[3]) )
Mrotation( om, orientation );
// Compute coordinates, texture coordinates:
for (i = 0, ci = 0; i < nSpine; ++i, ci+=nCrossSection) {
// Scale cross section
for (j = 0; j < nCrossSection; ++j) {
c[3*(ci+j)+0] = scale[0] * crossSection[ 2*j ];
c[3*(ci+j)+1] = 0.0;
c[3*(ci+j)+2] = scale[1] * crossSection[ 2*j+1 ];
}
// Compute Spine-aligned Cross-section Plane (SCP)
if (! spineStraight)
{
float S1[3], S2[3]; // Spine vectors [i,i-1] and [i,i+1]
int yi1, yi2, si1, s1i2, s2i2;
if (spineClosed && (i == 0 || i == nSpine-1))
{
yi1 = 3*(nSpine-2);
yi2 = 3;
si1 = 0;
s1i2 = 3*(nSpine-2);
s2i2 = 3;
}
else if (i == 0)
{
yi1 = 0;
yi2 = 3;
si1 = 3;
s1i2 = 0;
s2i2 = 6;
}
else if (i == nSpine-1)
{
yi1 = 3*(nSpine-2);
yi2 = 3*(nSpine-1);
si1 = 3*(nSpine-2);
s1i2 = 3*(nSpine-3);
s2i2 = 3*(nSpine-1);
}
else
{
yi1 = 3*(i-1);
yi2 = 3*(i+1);
si1 = 3*i;
s1i2 = 3*(i-1);
s2i2 = 3*(i+1);
}
Vdiff( Yscp, &spine[yi2], &spine[yi1] );
Vdiff( S1, &spine[s1i2], &spine[si1] );
Vdiff( S2, &spine[s2i2], &spine[si1] );
Vnorm( Yscp );
Vset(lastZ, Zscp); // Save last Zscp
Vcross( Zscp, S2, S1 );
float VlenZ = (float)Vlength(Zscp);
if ( VlenZ == 0.0 )
Vset(Zscp, lastZ);
else
Vscale( Zscp, 1.0f/VlenZ );
if ((i > 0) && (Vdot( Zscp, lastZ ) < 0.0))
Vscale( Zscp, -1.0 );
Vcross( Xscp, Yscp, Zscp );
}
// Rotate cross section into SCP
for (j = 0; j < nCrossSection; ++j) {
float cx, cy, cz;
cx = c[3*(ci+j)+0]*Xscp[0]+c[3*(ci+j)+1]*Yscp[0]+c[3*(ci+j)+2]*Zscp[0];
cy = c[3*(ci+j)+0]*Xscp[1]+c[3*(ci+j)+1]*Yscp[1]+c[3*(ci+j)+2]*Zscp[1];
cz = c[3*(ci+j)+0]*Xscp[2]+c[3*(ci+j)+1]*Yscp[2]+c[3*(ci+j)+2]*Zscp[2];
c[3*(ci+j)+0] = cx;
c[3*(ci+j)+1] = cy;
c[3*(ci+j)+2] = cz;
}
// Apply orientation
if (! FPZERO(orientation[3]) )
{
if (nOrientation > 1)
Mrotation( om, orientation );
for (j = 0; j < nCrossSection; ++j) {
float cx, cy, cz;
cx = (float)(c[3*(ci+j)+0]*om[0][0]+c[3*(ci+j)+1]*om[1][0]+c[3*(ci+j)+2]*om[2][0]);
cy = (float)(c[3*(ci+j)+0]*om[0][1]+c[3*(ci+j)+1]*om[1][1]+c[3*(ci+j)+2]*om[2][1]);
cz = (float)(c[3*(ci+j)+0]*om[0][2]+c[3*(ci+j)+1]*om[1][2]+c[3*(ci+j)+2]*om[2][2]);
c[3*(ci+j)+0] = cx;
c[3*(ci+j)+1] = cy;
c[3*(ci+j)+2] = cz;
}
}
// Translate cross section
for (j = 0; j < nCrossSection; ++j) {
c[3*(ci+j)+0] += spine[3*i+0];
c[3*(ci+j)+1] += spine[3*i+1];
c[3*(ci+j)+2] += spine[3*i+2];
// Texture coords
tc[3*(ci+j)+0] = ((float) j) / (nCrossSection-1);
tc[3*(ci+j)+1] = 1.0f - ((float) i) / (nSpine-1);
tc[3*(ci+j)+2] = 0.0f;
}
if (nScale > 1) scale += 2;
if (nOrientation > 1) orientation += 4;
}
// And compute face indices:
if (faces)
{
int polyIndex = 0;
for (i = 0, ci = 0; i < nSpine-1; ++i, ci+=nCrossSection) {
for (j = 0; j < nCrossSection-1; ++j) {
faces[polyIndex + 0] = ci+j;
faces[polyIndex + 1] = ci+j+1;
faces[polyIndex + 2] = ci+j+1 + nCrossSection;
faces[polyIndex + 3] = ci+j + nCrossSection;
faces[polyIndex + 4] = -1;
polyIndex += 5;
}
}
}
}
int
box::split_recurse(int *t)
{
// For a single triangle, orientation is easily determined.
// The major axis is parallel to the longest edge.
// The minor axis is normal to the triangle.
// The in-between axis is determine by these two.
// this->pR, this->d, and this->pT are set herein.
P = N = 0;
tri *ptr = RAPID_tri + t[0];
// Find the major axis: parallel to the longest edge.
double u12[3], u23[3], u31[3];
// First compute the squared-lengths of each edge
VmV(u12, ptr->p1, ptr->p2);
double d12 = VdotV(u12,u12);
VmV(u23, ptr->p2, ptr->p3);
double d23 = VdotV(u23,u23);
VmV(u31, ptr->p3, ptr->p1);
double d31 = VdotV(u31,u31);
// Find the edge of longest squared-length, normalize it to
// unit length, and put result into a0.
double a0[3];
double l;
if (d12 > d23)
{
if (d12 > d31)
{
l = 1.0 / sqrt(d12);
a0[0] = u12[0] * l;
a0[1] = u12[1] * l;
a0[2] = u12[2] * l;
}
else
{
l = 1.0 / sqrt(d31);
a0[0] = u31[0] * l;
a0[1] = u31[1] * l;
a0[2] = u31[2] * l;
}
}
else
{
if (d23 > d31)
{
l = 1.0 / sqrt(d23);
a0[0] = u23[0] * l;
a0[1] = u23[1] * l;
a0[2] = u23[2] * l;
}
else
{
l = 1.0 / sqrt(d31);
a0[0] = u31[0] * l;
a0[1] = u31[1] * l;
a0[2] = u31[2] * l;
}
}
// Now compute unit normal to triangle, and put into a2.
double a2[3];
VcrossV(a2, u12, u23);
l = 1.0 / Vlength(a2); a2[0] *= l; a2[1] *= l; a2[2] *= l;
// a1 is a2 cross a0.
double a1[3];
VcrossV(a1, a2, a0);
// Now make the columns of this->pR the vectors a0, a1, and a2.
pR[0][0] = a0[0]; pR[0][1] = a1[0]; pR[0][2] = a2[0];
pR[1][0] = a0[1]; pR[1][1] = a1[1]; pR[1][2] = a2[1];
pR[2][0] = a0[2]; pR[2][1] = a1[2]; pR[2][2] = a2[2];
// Now compute the maximum and minimum extents of each vertex
// along each of the box axes. From this we will compute the
// box center and box dimensions.
double minval[3], maxval[3];
double c[3];
MTxV(c, pR, ptr->p1);
minval[0] = maxval[0] = c[0];
minval[1] = maxval[1] = c[1];
minval[2] = maxval[2] = c[2];
MTxV(c, pR, ptr->p2);
minmax(minval[0], maxval[0], c[0]);
minmax(minval[1], maxval[1], c[1]);
minmax(minval[2], maxval[2], c[2]);
MTxV(c, pR, ptr->p3);
minmax(minval[0], maxval[0], c[0]);
minmax(minval[1], maxval[1], c[1]);
minmax(minval[2], maxval[2], c[2]);
// With the max and min data, determine the center point and dimensions
// of the box
c[0] = (minval[0] + maxval[0])*0.5;
c[1] = (minval[1] + maxval[1])*0.5;
c[2] = (minval[2] + maxval[2])*0.5;
pT[0] = c[0] * pR[0][0] + c[1] * pR[0][1] + c[2] * pR[0][2];
pT[1] = c[0] * pR[1][0] + c[1] * pR[1][1] + c[2] * pR[1][2];
pT[2] = c[0] * pR[2][0] + c[1] * pR[2][1] + c[2] * pR[2][2];
d[0] = (maxval[0] - minval[0])*0.5;
d[1] = (maxval[1] - minval[1])*0.5;
d[2] = (maxval[2] - minval[2])*0.5;
// Assign the one triangle to this box
trp = ptr;
return RAPID_OK;
}
