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