/**
* register triangles
*/
static void apply_transform(TransformNode* tnode, fVec3& pos, int scale_only = false)
{
static float fscale[3];
// scale
tnode->getScale(fscale);
pos(0) *= fscale[0];
pos(1) *= fscale[1];
pos(2) *= fscale[2];
// translation, rotation
if(!scale_only)
{
static fVec3 trans, tmp;
static fMat33 att;
static float fpos[3];
static float frot[4];
tnode->getTranslation(fpos);
tnode->getRotation(frot);
trans(0) = fpos[0];
trans(1) = fpos[1];
trans(2) = fpos[2];
att.rot2mat(fVec3(frot[0], frot[1], frot[2]), frot[3]);
tmp.mul(att, pos);
pos.add(trans, tmp);
}
}
TransformComponent::TransformComponent() : IGameObjectComponent( GameObjectComponent::Transform ),
m_changeTransform( false )
{
Mat4x4::Identity( &m_transform );
Mat4x4::Identity( &m_transformNoScale );
Mat4x4::Identity( &m_matScale );
Mat4x4::Identity( &m_matPos );
Mat4x4::Identity( &m_matRot );
m_scale = fVec3( 1.f, 1.f, 1.f );
Quaternion::Identity( &m_rot );
}
void pSubChain::col_disassembly_body()
{
// for space
if(!children[1]) return;
int i;
// compute final constraint force
static fVec KLf, Lf(6), Lf_temp[2], v(6), colf, colf_final(6);
KLf.resize(n_const);
Lf_temp[0].resize(6);
Lf_temp[1].resize(6);
colf.resize(n_const);
Lf_temp[0].zero();
Lf_temp[1].zero();
for(i=0; i<n_outer_joints; i++)
{
int org = outer_joints_origin[i];
int index = outer_joints_index[i];
fMat& Lambda_i = children[org]->Lambda[last_index[org]][index];
// first multiply and increment
v.mul(Lambda_i, children[org]->outer_joints[index]->colf_final);
Lf_temp[org] += v;
}
Lf.sub(Lf_temp[0], Lf_temp[1]);
// then extract rows
for(i=0; i<n_const; i++)
KLf(i) = Lf(const_index[i]);
#ifndef USE_DCA // TODO: allow DCA
colf.mul(Gamma_inv, KLf);
#endif
colf_temp -= colf;
colf_final.zero();
for(i=0; i<n_const; i++)
colf_final(const_index[i]) = colf_temp(i);
last_pjoints[0]->colf_final.set(colf_final);
last_pjoints[1]->colf_final.neg(colf_final);
// compute link velocities
last_pjoints[0]->vel_final.mul(children[0]->Lambda[last_index[0]][last_index[0]], last_pjoints[0]->colf_final);
last_pjoints[0]->vel_final += Lf_temp[0];
last_pjoints[0]->vel_final += children[0]->vel_temp[last_index[0]];
if(children[1])
{
last_pjoints[1]->vel_final.mul(children[1]->Lambda[last_index[1]][last_index[1]], last_pjoints[1]->colf_final);
last_pjoints[1]->vel_final += Lf_temp[1];
last_pjoints[1]->vel_final += children[1]->vel_temp[last_index[1]];
}
if(children[0] == children[1])
{
v.mul(children[1]->Lambda[last_index[0]][last_index[1]], last_pjoints[1]->colf_final);
last_pjoints[0]->vel_final += v;
v.mul(children[0]->Lambda[last_index[1]][last_index[0]], last_pjoints[0]->colf_final);
last_pjoints[1]->vel_final += v;
}
// compute joint velocity
v.sub(last_pjoints[0]->vel_final, last_pjoints[1]->vel_final);
// cerr << last_joint->name << ": v = " << tran(v) << endl;
switch(last_joint->j_type)
{
case JROTATE:
case JSLIDE:
last_joint->SetJointVel(v(axis));
break;
case JSPHERE:
last_joint->SetJointVel(fVec3(v(3), v(4), v(5)));
break;
case JFREE:
last_joint->SetJointVel(fVec3(v(0), v(1), v(2)), fVec3(v(3), v(4), v(5)));
break;
default:
break;
}
}
void pSubChain::disassembly_body()
{
// for space
if(!children[1]) return;
#ifdef VERBOSE
update_log << "disassembly_body" << endl;
#endif
#ifdef USE_MPI
if(sim->rank != rank) return;
if(parent && sim->rank != parent->rank)
{
#ifdef TIMING_CHECK
cerr << "[" << sim->rank << "] " << last_joint->name << ": recv force from " << parent->last_joint->name << " [" << parent->rank << "] t = " << MPI_Wtime()-update_start_time << endl;
#endif
parent->recv_force();
}
#endif
int i;
#ifdef TIMING_CHECK
if(children[1] && children[0] != children[1] && sim->rank != children[1]->rank)
cerr << "[" << sim->rank << "] " << last_joint->name << " enter disassembly t = " << MPI_Wtime()-update_start_time << endl;
#endif
// compute final constraint force
static fVec KLf, Lf(6), Lf_temp[2], v(6), f, f_final(6);
KLf.resize(n_const);
Lf_temp[0].resize(6);
Lf_temp[1].resize(6);
f.resize(n_const);
Lf_temp[0].zero();
Lf_temp[1].zero();
for(i=0; i<n_outer_joints; i++)
{
int org = outer_joints_origin[i];
int index = outer_joints_index[i];
fMat& Lambda_i = children[org]->Lambda[last_index[org]][index];
// first multiply and increment
// v.mul(Lambda_i, children[org]->outer_joints[index]->f_final);
v.mul(Lambda_i, outer_joints[i]->f_final);
#ifdef VERBOSE
// update_log << "children[" << org << "]->Lambda[" << last_index[org] << "][" << index << "] = " << Lambda_i << endl;
update_log << outer_joints[i]->joint->name << ": f_final[" << i << "] = " << tran(outer_joints[i]->f_final) << endl;
#endif
Lf_temp[org] += v;
}
Lf.sub(Lf_temp[0], Lf_temp[1]);
// update_log << "Lf_temp[0] = " << tran(Lf_temp[0]) << endl;
// update_log << "Lf_temp[1] = " << tran(Lf_temp[1]) << endl;
// all test codes removed on 02/09/2007
static fVec pp(6);
#ifndef USE_DCA
// new formulation
pp.add(da6, Lf);
f_final.mul(W, pp);
for(i=0; i<n_dof; i++)
f_final(joint_index[i]) += tau(i);
last_pjoints[0]->f_final.set(f_final);
last_pjoints[1]->f_final.neg(f_final);
v.mul(IW, pp);
for(i=0; i<n_dof; i++)
{
acc_final(i) = v(joint_index[i]);
}
last_joint->joint_f.set(fVec3(f_final(0), f_final(1), f_final(2)));
last_joint->joint_n.set(fVec3(f_final(3), f_final(4), f_final(5)));
#else // #ifndef USE_DCA (DCA test)
static fVec vp(6), svp;
svp.resize(n_dof);
pp.set(da6);
pp += Lf;
vp.mul(Vhat, pp);
for(i=0; i<n_dof; i++)
{
svp(i) = tau(i) + vp(joint_index[i]);
}
acc_final.lineq_posv(SVS, svp);
// cerr << "SVS = " << SVS << endl;
// cerr << "svp = " << tran(svp) << endl;
// cerr << "acc_final = " << tran(acc_final) << endl;
v.zero();
switch(last_joint->j_type)
{
case JROTATE:
case JSLIDE:
v(axis) = acc_final(0);
break;
case JSPHERE:
v(3) = acc_final(0);
v(4) = acc_final(1);
v(5) = acc_final(2);
break;
case JFREE:
v.set(acc_final);
break;
}
pp -= v;
f_final.mul(Vhat, pp);
last_pjoints[0]->f_final.neg(f_final);
last_pjoints[1]->f_final.set(f_final);
#endif
#ifndef USE_MPI
if(last_joint->t_given) {
switch(last_joint->j_type) {
case JROTATE:
case JSLIDE:
last_joint->SetJointAcc(v(axis));
// cerr << last_joint->name << ": " << v(axis) << endl;
break;
case JSPHERE:
last_joint->SetJointAcc(v(3), v(4), v(5));
break;
case JFREE:
last_joint->SetJointAcc(v(0), v(1), v(2), v(3), v(4), v(5));
#ifdef VERBOSE
update_log << last_joint->name << ": " << tran(v) << endl;
#endif
break;
default:
break;
}
}
#endif
#ifdef USE_MPI
if(children[0] && sim->rank != children[0]->rank)
{
#ifdef TIMING_CHECK
cerr << "[" << sim->rank << "] " << last_joint->name << ": send force to " << children[0]->last_joint->name << " [" << children[0]->rank << "] t = " << MPI_Wtime()-update_start_time << endl;
#endif
send_force(children[0]->rank);
}
if(children[1] && children[0] != children[1] && sim->rank != children[1]->rank)
{
#ifdef TIMING_CHECK
cerr << "[" << sim->rank << "] " << last_joint->name << ": send force to " << children[1]->last_joint->name << " [" << children[1]->rank << "] t = " << MPI_Wtime()-update_start_time << endl;
#endif
send_force(children[1]->rank);
}
#endif
}
