bool TET::restBaryCenter(const VEC3F& vert, QUATERNION& lambda) { VEC3F row[3]; row[0] = *(_restVertices[0]) - *(_restVertices[3]); row[1] = *(_restVertices[1]) - *(_restVertices[3]), row[2] = *(_restVertices[2]) - *(_restVertices[3]); MATRIX3 T; T << row[0][0], row[1][0], row[2][0], row[0][1], row[1][1], row[2][1], row[0][2], row[1][2], row[2][2]; VEC3F lambda0_3 = T.inverse() * (vert - *(_restVertices[3])); lambda.x() = lambda0_3[0]; lambda.y() = lambda0_3[1]; lambda.z() = lambda0_3[2]; lambda.w() = 1.0 - lambda.x() - lambda.y() - lambda.z(); if(!( 0 <= lambda.x() && lambda.x() <= 1 && 0 <= lambda.y() && lambda.y() <= 1 && 0 <= lambda.z() && lambda.z() <= 1 && 0 <= lambda.w() && lambda.w() <= 1) ) return false; return true; }
/// Multiplies a spatial vector void SPATIAL_RB_INERTIA::mult_spatial(const SVECTOR6& t, SVECTOR6& result) const { // get necessary components of t ORIGIN3 ttop(t.get_upper()); ORIGIN3 tbot(t.get_lower()); // do some precomputation MATRIX3 hx = MATRIX3::skew_symmetric(h); MATRIX3 hxm = MATRIX3::skew_symmetric(h*m); // Featherstone's code uses this format: // J-hx*hx*m hx*m // -hx*m mI // our format is: // -hx*m mI // J-hx*hx*m hx*m // compute result VECTOR3 rtop(tbot*m + hxm.transpose_mult(ttop), pose); VECTOR3 rbot(J*ttop-(hx*(hxm*ttop)) + hxm*tbot, pose); result.pose = pose; result.set_upper(rtop); result.set_lower(rbot); }
void SPATIAL_RB_INERTIA::inverse_mult_spatial(const SVECTOR6& w, const MATRIX3& iJ, const MATRIX3& hx, const MATRIX3& hxiJ, REAL inv_m, SVECTOR6& result) const { // get the components of the force ORIGIN3 top(w.get_upper()); ORIGIN3 bot(w.get_lower()); // do the arithmetic VECTOR3 ttop(hxiJ.transpose_mult(top) + iJ*bot, pose); VECTOR3 tbot(top*inv_m + hx*(hxiJ.transpose_mult(top)) + hxiJ.mult(bot), pose); result.pose = pose; result.set_upper(ttop); result.set_lower(tbot); }
MATRIX3 TET::G() { VEC3F row[3]; row[0] = *(vertices[0]) - *(vertices[3]); row[1] = *(vertices[1]) - *(vertices[3]), row[2] = *(vertices[2]) - *(vertices[3]); MATRIX3 T; T << row[0][0], row[1][0], row[2][0], row[0][1], row[1][1], row[2][1], row[0][2], row[1][2], row[2][2]; return T.transpose().inverse(); }
// return m0 * m1 MATRIX3 operator *(const MATRIX3 & m0, const MATRIX3 & m1) { MATRIX3 result; for (int i = 0; i < m0.Dimension(); i++) for (int j = 0; j < m0.Dimension(); j++) { result[i][j] = 0; for (int k = 0; k < m0.Dimension(); k++) result[i][j] += m0(i,k) * m1(k,j); }; return(result); }
MATRIX3 operator*(const MATRIX3& n, const MATRIX3& m) { MATRIX3 A; for(int i=0;i<3;i++) for(int j=0;j<3;j++) A(i,j) = n[i]*m.col(j); return A; }
int CurvilinearGrid:: inverse_jacobian_t(MATRIX3& mi, double r, double s, double t) { VECTOR3 dxyzdr = xyz_r + s*xyz_rs + t*xyz_rt + s*t*xyz_rst; VECTOR3 dxyzds = xyz_s + r*xyz_rs + t*xyz_st + r*t*xyz_rst; VECTOR3 dxyzdt = xyz_t + r*xyz_rt + s*xyz_st + r*s*xyz_rst; MATRIX3 m(dxyzdr, dxyzds, dxyzdt); MATRIX3 tm = m.transpose(); tm.inverse(mi); return 1; }
/// Setup the transform from one joint to another void FIXEDJOINT::setup_joint() { const unsigned X = 0, Y = 1, Z = 2; shared_ptr<const POSE3> GLOBAL; // get the two poses shared_ptr<const POSE3> inboard = get_inboard_pose(); shared_ptr<const POSE3> outboard = get_outboard_pose(); if (!inboard || !outboard) return; // get the rotation matrices MATRIX3 Ri = inboard->q; MATRIX3 Ro = outboard->q; // TODO: fix this // compute the relative transform /* MATRIX3 Rrel = MATRIX3::transpose(Ro) * Ri; _T->q = Rrel; _T->x.set_zero(); _T->source = ; // compute the vector from the inner link to the outer link in inner link // frame VECTOR3 ox(outboard->x, To); // _ui = inboard->inverse_transform(ox); */ // compute the constant orientation term _rconst[X] = VECTOR3(Ri.get_row(X), GLOBAL).dot(VECTOR3(Ro.get_row(X), GLOBAL)); _rconst[Y] = VECTOR3(Ri.get_row(Y), GLOBAL).dot(VECTOR3(Ro.get_row(Y), GLOBAL)); _rconst[Z] = VECTOR3(Ri.get_row(Z), GLOBAL).dot(VECTOR3(Ro.get_row(Z), GLOBAL)); }
/// Multiplies a spatial vector void SPATIAL_RB_INERTIA::mult_spatial(const SVECTOR6& t, const MATRIX3& hxm, const MATRIX3& Jmod, SVECTOR6& result) const { // get necessary components of t ORIGIN3 ttop(t.get_upper()); ORIGIN3 tbot(t.get_lower()); // compute result VECTOR3 rtop(tbot*m + hxm.transpose_mult(ttop), pose); VECTOR3 rbot(Jmod*ttop + hxm*tbot, pose); result.pose = pose; result.set_upper(rtop); result.set_lower(rbot); }
int main(int argc, char **argv) { printf("Usage: computeFTLE flowmap.raw w h d RES\n"); int w = atoi(argv[2]), h = atoi(argv[3]), d = atoi(argv[4]); float RES = atof(argv[5]); printf("Opening file %s, w h d: %d %d %d\n", argv[1], w, h, d); vector<VECTOR3> offset(w*h*d); FILE *fp = fopen(argv[1], "rb"); fread(&offset[0], w*h*d, 12, fp); fclose(fp); if (0) // now we generate flowmap { int x,y,z, count=0; for (z=0; z<d; z++) for (y=0; y<h; y++) for (x=0; x<w; x++) { offset[count] = offset[count] + VECTOR3(x*RES, y*RES, z*RES); //printf("offset: %f %f %f\n", offset[count][0], offset[count][1], offset[count][2]); count ++; } } // use osuflow OSUFlow *osuflow = new OSUFlow(); float minB[3] = {0,0,0}, maxB[3]; maxB[0] = w-1; maxB[1] = h-1; maxB[2] = d-1; osuflow->CreateStaticFlowField((float *)&offset[0], w, h, d, minB, maxB); CVectorField *field = osuflow->GetFlowField(); // FTLE vector<float> ftle(w*h*d); int x,y,z, count=0; for (z=0; z<d; z++) { for (y=0; y<h; y++) for (x=0; x<w; x++) { MATRIX3 jac = field->Jacobian(VECTOR3(x,y,z)), jsquare; // TODO: delta in Jac. computation jac = jac * (1/RES); jsquare = jac.transpose() * jac; float m[3][3], eigenvalues[3]; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { m[i][j] = jsquare(i, j); } } compute_eigenvalues(m, eigenvalues); float max_eig = max(eigenvalues[0], max(eigenvalues[1], eigenvalues[2])); //printf("%f\n", max_eig); ftle[count++] = log(max_eig)*.5; } printf("z=%d\n", z); } fp = fopen("ftle.raw", "wb"); fwrite(&ftle[0], w*h*d, sizeof(float), fp); fclose(fp); printf("output: ftle.raw\n"); return 0; }