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; }