/*
This function sets rot_parent_current data member.
Rotation from this bone local coordinate system
to the coordinate system of its parent
*/
void Skeleton::compute_rotation_parent_child(Bone *parent, Bone *child)
{
double Rx[4][4], Ry[4][4], Rz[4][4], tmp[4][4], tmp1[4][4], tmp2[4][4];
if(child != NULL)
{
// The following openGL rotations are pre-calculated and saved in the orientation matrix.
//
// glRotatef(-inboard->axis_x, 1., 0., 0.);
// glRotatef(-inboard->axis_y, 0., 1, 0.);
// glRotatef(-inboard->axis_z, 0., 0., 1.);
// glRotatef(outboard->axis_z, 0., 0., 1.);
// glRotatef(outboard->axis_y, 0., 1, 0.);
// glRotatef(outboard->axis_x, 1., 0., 0.);
rotationZ(Rz, -parent->axis_z);
rotationY(Ry, -parent->axis_y);
rotationX(Rx, -parent->axis_x);
matrix_mult(Rx, Ry, tmp);
matrix_mult(tmp, Rz, tmp1);
rotationZ(Rz, child->axis_z);
rotationY(Ry, child->axis_y);
rotationX(Rx, child->axis_x);
matrix_mult(Rz, Ry, tmp);
matrix_mult(tmp, Rx, tmp2);
matrix_mult(tmp1, tmp2, tmp);
matrix_transpose(tmp, child->rot_parent_current);
}
}
void Computation::computeGeneralCenterOfMass() {
double translation[3], rotation[3];
double R[4][4],Rx[4][4],Ry[4][4],Rz[4][4];
for (int i = 0; i < numOfSkeletons && i < MAX_SKELS; i++)
{
totalMass = 0.0; //reset total Mass
double transform[4][4];
identity(transform); // reset transform matrix to identity
//store previous position of gcm
m_pSkeletonList[i]->cm_prev[0] = m_pSkeletonList[i]->cm[0];
m_pSkeletonList[i]->cm_prev[1] = m_pSkeletonList[i]->cm[1];
m_pSkeletonList[i]->cm_prev[2] = m_pSkeletonList[i]->cm[2];
m_pSkeletonList[i]->cm[0] = 0.0;
m_pSkeletonList[i]->cm[1] = 0.0;
m_pSkeletonList[i]->cm[2] = 0.0;
if (m_pMassDistributionList[i] != NULL) {
m_pSkeletonList[i]->GetTranslation(translation);
m_pSkeletonList[i]->GetRotationAngle(rotation);
//creating Rotation matrix for initial rotation of Skeleton
rotationX(Rx, rotation[0]);
rotationY(Ry, rotation[1]);
rotationZ(Rz, rotation[2]);
matrix4_mult(Rz, Ry, R);
matrix4_mult(R, Rx, R);
matrix4_mult(transform, R, transform);
transform[0][3] += (MOCAP_SCALE*translation [0]);
transform[1][3] += (MOCAP_SCALE*translation [1]);
transform[2][3] += (MOCAP_SCALE*translation [2]);
traverse(m_pSkeletonList[i]->getRoot(), i, transform, 'g');
m_pSkeletonList[i]->cm[0] /= (totalMass);
m_pSkeletonList[i]->cm[1] /= (totalMass);
m_pSkeletonList[i]->cm[2] /= (totalMass);
m_pSkeletonList[i]->totalMass = totalMass;
} else {
printf("Entry of m_pMotionList[%d] is empty.\n", i);
//exit(0);
}
}
}
void RenderLine2D(Shader *sh, Primitive prim, const float3 &v1, const float3 &v2, float thickness)
{
auto v = (v2 - v1) / 2;
auto len = length(v);
auto vnorm = v / len;
auto trans = translation(v1 + v) *
rotationZ(vnorm.xy()) *
float4x4(float4(len, thickness / 2, 1, 1));
RenderQuad(sh, prim, true, trans);
}
STARTDECL(ph_render) ()
{
CheckPhysics();
auto oldobject2view = object2view;
auto oldcolor = curcolor;
for (b2Body *body = world->GetBodyList(); body; body = body->GetNext())
{
auto pos = body->GetPosition();
auto mat = translation(float3(pos.x, pos.y, 0)) * rotationZ(body->GetAngle());
object2view = oldobject2view * mat;
for (b2Fixture *fixture = body->GetFixtureList(); fixture; fixture = fixture->GetNext())
{
auto shapetype = fixture->GetType();
auto r = (Renderable *)fixture->GetUserData();
curcolor = r->color;
r->Set();
switch (shapetype)
{
case b2Shape::e_polygon:
{
auto polyshape = (b2PolygonShape *)fixture->GetShape();
RenderArray(PRIM_FAN, polyshape->m_count, "pn", sizeof(b2Vec2), polyshape->m_vertices, NULL,
sizeof(b2Vec2), polyshape->m_normals);
break;
}
case b2Shape::e_circle:
{
// FIXME: instead maybe cache a circle verts somewhere.. though should maxverts be changable?
const int maxverts = 20;
struct PhVert { float2 pos; float2 norm; } phverts[maxverts];
auto polyshape = (b2CircleShape *)fixture->GetShape();
float step = PI * 2 / maxverts;
for (int i = 0; i < maxverts; i++)
{
auto pos = float2(sinf(i * step + 1), cosf(i * step + 1));
phverts[i].pos = pos * polyshape->m_radius + *(float2 *)&polyshape->m_p;
phverts[i].norm = pos;
}
RenderArray(PRIM_FAN, maxverts, "pn", sizeof(PhVert), phverts, NULL);
break;
}
case b2Shape::e_edge:
case b2Shape::e_chain:
case b2Shape::e_typeCount:
assert(0);
break;
}
}
}
object2view = oldobject2view;
curcolor = oldcolor;
return Value();
}
/*
Rotate vector v by a, b, c in X,Y,Z order.
v_out = Rz(c)*Ry(b)*Rx(a)*v_in
*/
void vector_rotationXYZ(double *v, double a, double b, double c)
{
double Rx[4][4], Ry[4][4], Rz[4][4];
//Rz is a rotation matrix about Z axis by angle c, same for Ry and Rx
rotationZ(Rz, c);
rotationY(Ry, b);
rotationX(Rx, a);
//Matrix vector multiplication to generate the output vector v.
matrix_transform_affine(Rz, v[0], v[1], v[2], v);
matrix_transform_affine(Ry, v[0], v[1], v[2], v);
matrix_transform_affine(Rx, v[0], v[1], v[2], v);
}
void ToyRotationMatrix::rotateZ(float degree) {
// TODO: Throw exception here
if (Rows!=4 || Columns!=4) {
assert(false);
}
DegreeZ=degree;
float rotationElements[16] = { cos(degree), -sin(degree), 0.0, 0.0,
sin(degree), cos(degree), 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0};
float *rotationElementsP = new float[16];
memcpy(rotationElementsP, &rotationElements, 16*sizeof(float));
ToyMatrix<float> rotationZ(4, 4, rotationElementsP);
(*this) *= rotationZ;
}
void Engine::Transform( iAVec3f * vec )
{
//!@note rotations and translations are inversed, because we rotating plane and origin instead of object
float
irotX = -rotX,
irotY = -rotY,
irotZ = -rotZ;
iAVec3f iposition = -position;
iAMat4 mrotx, mroty, mrotz;
mrotz = rotationZ(irotZ);
mroty = rotation(mrotz*iAVec3f(0,1,0), irotY);
mrotx = rotation(mrotz*iAVec3f(1,0,0), irotX);
iAMat4 rot_mat = mrotx*mroty*mrotz;
//if plate rotation is desired, should do translation after rotation
(*vec) = rot_mat*((*vec)+iposition);
}
// -----------------------------------------------------------
// Engine::InitRender
// Initializes the renderer, by resetting the line / tile
// counters and precalculating some values
// -----------------------------------------------------------
void Engine::InitRender(iAVec3f * vp_corners, iAVec3f * vp_delta, iAVec3f * o)
{
//!@note rotations and translations are inversed, because we rotating plane and origin instead of object
float
irotX = -rotX,
irotY = -rotY,
irotZ = -rotZ;
iAVec3f iposition = -position;
iAMat4 mrotx, mroty, mrotz;
mrotz = rotationZ(irotZ);
mroty = rotation(mrotz*iAVec3f(0,1,0), irotY);
mrotx = rotation(mrotz*iAVec3f(1,0,0), irotX);
iAMat4 rot_mat = mrotx*mroty*mrotz;
//iAMat4 rot_mat = rotationX(irotX)*rotationY(irotY)*rotationZ(irotZ);
//if plate rotation is desired, should do translation after rotation
vp_corners[0] = rot_mat*(iAVec3f(m_WX1, m_WY1, m_PLANE_Z)+iposition);
vp_corners[1] = rot_mat*(iAVec3f(m_WX2, m_WY2, m_PLANE_Z)+iposition);
(*o) = rot_mat*(iAVec3f( 0, 0, m_ORIGIN_Z )+iposition);
//no translations here
vp_delta[0] = rot_mat*iAVec3f(m_DX, 0, 0);
vp_delta[1] = rot_mat*iAVec3f(0, m_DY, 0);
}
// loop through all bones to calculate local coordinate's direction vector and relative orientation
void Skeleton::ComputeRotationToParentCoordSystem(Bone *bone)
{
int i;
double Rx[4][4], Ry[4][4], Rz[4][4], tmp[4][4], tmp2[4][4];
//Compute rot_parent_current for the root
//Compute tmp2, a matrix containing root
//joint local coordinate system orientation
int root = Skeleton::getRootIndex();
rotationZ(Rz, bone[root].axis_z);
rotationY(Ry, bone[root].axis_y);
rotationX(Rx, bone[root].axis_x);
matrix_mult(Rz, Ry, tmp);
matrix_mult(tmp, Rx, tmp2);
//set bone[root].rot_parent_current to transpose of tmp2
matrix_transpose(tmp2, bone[root].rot_parent_current);
//Compute rot_parent_current for all other bones
int numbones = numBonesInSkel(bone[0]);
for(i=0; i<numbones; i++)
{
if(bone[i].child != NULL)
{
compute_rotation_parent_child(&bone[i], bone[i].child);
// compute parent child siblings...
Bone * tmp = NULL;
if (bone[i].child != NULL) tmp = (bone[i].child)->sibling;
while (tmp != NULL)
{
compute_rotation_parent_child(&bone[i], tmp);
tmp = tmp->sibling;
}
}
}
}
void Computation::traverse(Bone * ptr, int skelNum, double transform[4][4], char c){
if (ptr != NULL) {
double Rx[4][4], Ry[4][4], Rz[4][4], M[4][4]; //store rotation matrices.
double transformBackUp[4][4];
double translation[4][4];
double C[4][4], Cinv[4][4];
matrix4_copy(transform, transformBackUp);
//create homogeneous transformation to next frame.
identity(M);
identity(translation);
identity(Rx);
identity(Ry);
identity(Rz);
// compute C
rotationZ(Rz, ptr->axis_z);
rotationY(Ry, ptr->axis_y);
rotationX(Rx, ptr->axis_x);
matrix4_mult(Rz, Ry, C);
matrix4_mult(C, Rx, C);
// compute M
rotationX(Rx, (ptr->rx));
rotationY(Ry, (ptr->ry));
rotationZ(Rz, (ptr->rz));
matrix4_mult(Rz, Ry, M);
matrix4_mult(M, Rx, M);
M[0][3] += ptr->tx;
M[1][3] += ptr->ty;
M[2][3] += ptr->tz;
matrix4_mult(transform, C, transform);
matrix4_mult(transform, M, transform);
if(c == 'g') computeCM(ptr, skelNum, transform);
if(c == 'h') computePos(ptr, transform);
translation[0][3] = ptr->dir[0]*ptr->length;
translation[1][3] = ptr->dir[1]*ptr->length;
translation[2][3] = ptr->dir[2]*ptr->length;
matrix4_mult(transform, translation, transform);
matrix4_transpose(C, Cinv);
matrix4_mult(transform, Cinv, transform);
traverse(ptr->child, skelNum, transform, c);
traverse(ptr->sibling, skelNum, transformBackUp, c);
}
}
//Computes the angular momentum about the general center of mass
void Computation::computeAngularMomentum() {
double translation[3], rotation[3];
double R[4][4],Rx[4][4],Ry[4][4],Rz[4][4];
double mean_w[3];
Bone * root;
Mass * mass;
if(m_pSkeletonList != NULL)
{
for (int i = 0; i < numOfSkeletons && i < MAX_SKELS; i++)
{
double transform[4][4];
identity(transform);
double r_i_cm[m_pSkeletonList[i]->NUM_BONES_IN_ASF_FILE][3]; //stores previous position of local center of mass in global cs
root = m_pSkeletonList[i]->getRoot();
if(m_pMassDistributionList[i] != NULL) {
//reset angular momentum
m_pSkeletonList[i]->H[0] = 0.;
m_pSkeletonList[i]->H[1] = 0.;
m_pSkeletonList[i]->H[2] = 0.;
//store old local cm in global cs of each bone:
for( int k = 0; k < m_pSkeletonList[i]->NUM_BONES_IN_ASF_FILE; k++)
{
//store previous position of local cm in global cs in r_i_cm
r_i_cm[k][0] = m_pSkeletonList[i]->getBone(root, k)->r_i[0];
r_i_cm[k][1] = m_pSkeletonList[i]->getBone(root, k)->r_i[1];
r_i_cm[k][2] = m_pSkeletonList[i]->getBone(root, k)->r_i[2];
}
//compute current position of lcm in global cs
m_pSkeletonList[i]->GetTranslation(translation);
m_pSkeletonList[i]->GetRotationAngle(rotation);
//creating Rotation matrix for initial rotation of Skeleton
rotationX(Rx, rotation[0]);
rotationY(Ry, rotation[1]);
rotationZ(Rz, rotation[2]);
matrix4_mult(Rz, Ry, R);
matrix4_mult(R, Rx, R);
matrix4_mult(transform, R, transform);
transform[0][3] += (MOCAP_SCALE*translation [0]);
transform[1][3] += (MOCAP_SCALE*translation [1]);
transform[2][3] += (MOCAP_SCALE*translation [2]);
traverse(root, i, transform, 'h');
//TODO delete
mean_w[0] = 0;
mean_w[1] = 0;
mean_w[2] = 0;
for(int j = 0; j < m_pSkeletonList[i]->NUM_BONES_IN_ASF_FILE; j++) { //for every bone compute: (r_i - r_cm) x m_i(v_i - v_cm) + I_i*w_i
double rel_pos[3], v_i[3], v_cm[3], v_rel[3], I_G[3][3], w_i[3], local_inertia[3], cross[3], S[3][3], S_transpose[3][3];
Bone * bone;
bone = m_pSkeletonList[i]->getBone(root, j);
if(m_pMassDistributionList[i]->getMass(bone->name) != NULL) {
mass = m_pMassDistributionList[i]->getMass(bone->name);
//rel_pos = r_i - r_cm
rel_pos[0] = bone->r_i[0] - m_pSkeletonList[i]->cm[0];
rel_pos[1] = bone->r_i[1] - m_pSkeletonList[i]->cm[1];
rel_pos[2] = bone->r_i[2] - m_pSkeletonList[i]->cm[2];
//v_i = r_i - r_i_cm
v_i[0] = (bone->r_i[0] - r_i_cm[j][0]);
v_i[1] = (bone->r_i[1] - r_i_cm[j][1]);
v_i[2] = (bone->r_i[2] - r_i_cm[j][2]);
//if v_i is zero switch flag
checkLegSwing(v_i, bone, i);
// if(mass->mass != 0 && ((strcmp(mass->segName, "rfoot") == 0) || (strcmp(mass->segName, "lfoot") == 0))) printf("v_i_%s: %f %f %f\n",mass->segName, v_i[0], v_i[1], v_i[2]);
//v_cm = r_cm - r_cm_prev
v_cm[0] = (m_pSkeletonList[i]->cm[0] - m_pSkeletonList[i]->cm_prev[0]);
v_cm[1] = (m_pSkeletonList[i]->cm[1] - m_pSkeletonList[i]->cm_prev[1]);
v_cm[2] = (m_pSkeletonList[i]->cm[2] - m_pSkeletonList[i]->cm_prev[2]);
//v_rel = v_i - v_cm
v_rel[0] = v_i[0] - v_cm[0];
v_rel[1] = v_i[1] - v_cm[1];
v_rel[2] = v_i[2] - v_cm[2];
//Inertia tensor
double Ri[4][4], Ri_transpose[4][4], I_i[4][4];
//rotation from global to local
rotationX(Rx, -bone->axis_x);
rotationY(Ry, -bone->axis_y);
rotationZ(Rz, -bone->axis_z);
matrix4_mult(Rz, Ry, Ri);
matrix4_mult(Ri, Rx, Ri);
matrix4_transpose(Ri, Ri_transpose);
I_i[0][0] = mass->Ixx;
I_i[0][1] = I_i[1][0] =(-1)*mass->Ixy;
I_i[0][2] = I_i[2][0] = (-1)*mass->Ixz;
I_i[1][1] = mass->Iyy;
I_i[1][2] = I_i[2][1] = (-1)*mass->Iyz;
I_i[2][2] = mass->Izz;
I_i[0][3] = I_i[1][3] = I_i[2][3] = 0;
I_i[3][0] = I_i[3][1] = I_i[3][2] = 0;
I_i[3][3] = 1.0;
//~I_G = R_i^T * I_i * R_i
//~I_G is inertia tensor relative to bone's cm in rotation of global cs.
matrix4_mult(Ri_transpose, I_i, I_i);
matrix4_mult(I_i, Ri, I_i);
//parallel axes theorem
// I_G = I_i + m_i * S^T(rel_pos)* S(rel_pos) with S is skew matrix for cross product
cross_matrix(rel_pos, S); //works
matrix3_transpose(S, S_transpose);//works
matrix3_mult(S_transpose, S, S); //works
matrix3_scalar_mult(S, mass->mass); //works
//copy entries into I_G
identity3(I_G);
for (int x = 0; x < 3; x++)
for (int y = 0; y < 3; y++)
I_G[x][y] = I_i[x][y] + S[x][y];
//w_i = R_i^T * ({rx,ry,rz} - {rx_prev, ry_prev, rz_prev});
w_i[0] = (bone->rx - bone->rx_prev);
w_i[1] = (bone->ry - bone->ry_prev);
w_i[2] = (bone->rz - bone->rz_prev);
//clamping
if(absolute_value(w_i[0]) > 1.5) w_i[0] = 0;
if(absolute_value(w_i[1]) > 1.5) w_i[1] = 0;
if(absolute_value(w_i[2]) > 1.5) w_i[2] = 0;
mean_w[0] += absolute_value(w_i[0]);
mean_w[1] += absolute_value(w_i[1]);
mean_w[2] += absolute_value(w_i[2]);
vector_rotationXYZ(w_i, bone->axis_x, bone->axis_y, bone->axis_z);
//local_inertia = I_i*w_i
matrix3_v3_mult(I_G,w_i, local_inertia);
//cross = (r_i - r_cm) x m_i(v_i - v_cm)
v_rel[0] *= m_pMassDistributionList[i]->getMass(bone->name)->mass;
v_rel[1] *= m_pMassDistributionList[i]->getMass(bone->name)->mass;
v_rel[2] *= m_pMassDistributionList[i]->getMass(bone->name)->mass;
v3_cross(rel_pos, v_rel, cross);
//H is sum of angular momentum of every bone
m_pSkeletonList[i]->H[0] += (cross[0] + local_inertia[0]);
m_pSkeletonList[i]->H[1] += (cross[1] + local_inertia[1]);
m_pSkeletonList[i]->H[2] += (cross[2] + local_inertia[2]);
} //if end
}//for bones end
setLegSwing(i);
mean_w[0] /= m_pSkeletonList[i]->NUM_BONES_IN_ASF_FILE;
mean_w[1] /= m_pSkeletonList[i]->NUM_BONES_IN_ASF_FILE;
mean_w[2] /= m_pSkeletonList[i]->NUM_BONES_IN_ASF_FILE;
//normalization N=M*H*V
// to make H dimensionless, divide by subject's height (in m), subject's mass (in kg) and subject's average velocity(0.012 m/frame or 1.44 m/s)
double n = m_pSkeletonList[i]->totalMass*m_pSkeletonList[i]->height*(1.44);
m_pSkeletonList[i]->H[0] /= n;
m_pSkeletonList[i]->H[1] /= n;
m_pSkeletonList[i]->H[2] /= n;
//printf("mean w values: %f %f %f\n", mean_w[0], mean_w[1], mean_w[2]);
//printf("angular momentum: %f %f %f\n", m_pSkeletonList[i]->H[0],m_pSkeletonList[i]->H[1],m_pSkeletonList[i]->H[2]);
//printf("position cm: %f %f %f\n", m_pSkeletonList[i]->cm[0], m_pSkeletonList[i]->cm[1], m_pSkeletonList[i]->cm[2]);
}//if end
}// for Skeletons end
}
}
Eigen::MatrixXd MainWindow::rpy2rotation( double roll, double pitch, double yaw )
{
Eigen::MatrixXd _rotation = rotationZ( yaw ) * rotationY( pitch ) * rotationX( roll );
return _rotation;
}
STARTDECL(gl_rotate_z) (Value &angle, Value &body)
{
auto a = transangle(angle);
return pushtrans(rotationZ(a), rotationZ(a * float2(1, -1)), body);
}
// [7/30/2008 zhangxiang]
void sgNode::roll(const Radian &aAngle, int aRelativeTo /* = TS_LOCAL */){
rotationZ(aAngle, aRelativeTo);
}
Matrix Matrix::rotation(const long double &angleX, const long double &angleY, const long double &angleZ)
{
return rotationX(angleX) * rotationY(angleY) * rotationZ(angleZ);
}
