AA_API void
aa_tf_qutr_mulc( const double A[AA_RESTRICT 7], const double B[AA_RESTRICT 7], double C[AA_RESTRICT 7] )
{
double x[7];
aa_tf_qutr_conj(B,x);
aa_tf_qutr_mul(A,x,C);
}
void tf_conj(double E[2][7],
double S[2][8] )
{
double S2[8], SE[7], E2[8];
aa_tf_duqu_conj(S[0], S2);
aa_tf_qutr_conj(E[0], E2);
aa_tf_duqu2qutr(S2, SE);
aveq( "duqu/qutr conj", 7, E2, SE, 1e-7 );
aa_tf_duqu_mul(S[0], S[1], S2);
aa_tf_qutr_mul(E[0], E[1], E2);
aa_tf_duqu2qutr(S2, SE);
aveq( "duqu/qutr mul", 7, E2, SE, 1e-7 );
aa_tf_duqu_mulc(S[0], S[1], S2);
aa_tf_qutr_mulc(E[0], E[1], E2);
aa_tf_duqu2qutr(S2, SE);
aveq( "duqu/qutr mulc", 7, E2, SE, 1e-7 );
aa_tf_duqu_cmul(S[0], S[1], S2);
aa_tf_qutr_cmul(E[0], E[1], E2);
aa_tf_duqu2qutr(S2, SE);
aveq( "duqu/qutr cmul", 7, E2, SE, 1e-7 );
}
AA_API void aa_rx_sg_tf
( const struct aa_rx_sg *scene_graph,
size_t n_q, const double *q,
size_t n_tf,
double *TF_rel, size_t ld_rel,
double *TF_abs, size_t ld_abs )
{
aa_rx_sg_ensure_clean_frames( scene_graph );
amino::SceneGraph *sg = scene_graph->sg;
size_t i_frame = 0;
for( size_t i_rel = 0, i_abs = 0;
i_frame < n_tf && i_frame < sg->frames.size();
i_frame++, i_rel += ld_rel, i_abs += ld_abs )
{
amino::SceneFrame *f = sg->frames[(aa_rx_frame_id)i_frame];
double *E_rel = TF_rel + i_rel;
double *E_abs = TF_abs + i_abs;
// compute relative
f->tf_rel( q, E_rel );
// chain to global
if( f->in_global() ) {
// TODO: can we somehow get rid of this branch?
// maybe a separate type for global frames
AA_MEM_CPY(E_abs, E_rel, 7);
} else {
assert( f->parent_id < (ssize_t)i_frame );
double *E_abs_parent = TF_abs + (ld_abs * f->parent_id);;
aa_tf_qutr_mul(E_abs_parent, E_rel, E_abs);
}
}
}
static void chain(double E[2][7],
double S[2][8],
double T[2][12] ) {
double E3[7];
// rotation
{
double q3[4]={0};
double R3[9]={0};
double qc[4]={0};
aa_tf_qmul( E[0], E[1], q3 );
aa_tf_9mul( T[0], T[1], R3 );
aa_tf_rotmat2quat( R3, qc );
aa_tf_qminimize( q3 );
aa_tf_qminimize( qc );
aveq("chain-rot q/9", 4, q3, qc, 1e-6 );
}
// Transformation
aa_tf_qutr_mul( E[0], E[1], E3 );
aa_tf_qminimize( E3 );
// duqu
{
double S3[8]={0}, Ec[7]={0};
aa_tf_duqu_mul( S[0], S[1], S3 );
aa_tf_duqu2qutr( S3, Ec );
aa_tf_qminimize( Ec );
aveq("chain-tf qutr/duqu", 7, E3, Ec, 1e-6 );
}
// qutr_norm
{
double N3[7]={0};
aa_tf_qutr_mulnorm(E[0], E[1], N3);
aa_tf_qminimize( N3 );
aveq("chain-tf qutr/qutrnorm", 7, E3, N3, 1e-6 );
}
// tfmat
{
double T3[12]={0}, Ec[7]={0};
aa_tf_12chain( T[0], T[1], T3 );
aa_tf_tfmat2qutr( T3, Ec );
aa_tf_qminimize( Ec );
aveq("chain-tf qutr/tfmat", 7, E3, Ec, 1e-6 );
}
}
int
aa_rx_cl_check( struct aa_rx_cl *cl,
size_t n_tf,
const double *TF, size_t ldTF,
struct aa_rx_cl_set *cl_set )
{
/* Update Transforms */
for( auto itr = cl->objects->begin();
itr != cl->objects->end();
itr++ )
{
fcl::CollisionObject *obj = *itr;
aa_rx_frame_id id = (intptr_t) obj->getUserData();
const double *TF_obj = TF+id*ldTF;
enum aa_rx_geom_shape shape_type;
struct aa_rx_geom *geom = (struct aa_rx_geom*)obj->collisionGeometry()->getUserData();
void *shape_ = aa_rx_geom_shape( geom, &shape_type);
/* Special case cylinders.
* Amino cylinders extend in +Z
* FCL cylinders extend in both +/- Z.
*/
if( AA_RX_CYLINDER == shape_type ) {
struct aa_rx_shape_cylinder *shape = (struct aa_rx_shape_cylinder *) shape_;
double E[7] = {0,0,0,1, 0,0, shape->height/2};
double E1[7];
aa_tf_qutr_mul(TF_obj, E, E1);
obj->setTransform(amino::fcl::qutr2fcltf(E1));
} else {
obj->setTransform( amino::fcl::qutr2fcltf(TF_obj) );
}
}
cl->manager->update();
/* Check Collision */
struct cl_check_data data;
data.result = 0;
data.cl = cl;
data.cl_set = cl_set;
cl->manager->collide( &data, cl_check_callback );
return data.result;
}
AA_API void aa_rx_sg_tf_update
( const struct aa_rx_sg *scene_graph,
size_t n_q,
const double *q0,
const double *q,
size_t n_tf,
const double *TF_rel0, size_t ld_rel0,
const double *TF_abs0, size_t ld_abs0,
double *TF_rel, size_t ld_rel,
double *TF_abs, size_t ld_abs )
{
aa_rx_sg_ensure_clean_frames( scene_graph );
amino::SceneGraph *sg = scene_graph->sg;
bool updated[sg->frames.size()];
size_t i_frame = 0;
for( size_t i_rel0 = 0, i_abs0 = 0,
i_rel = 0, i_abs = 0;
i_frame < n_tf && i_frame < sg->frames.size();
i_frame++, i_rel += ld_rel, i_abs += ld_abs,
i_rel0 += ld_rel0, i_abs0 += ld_abs0
)
{
amino::SceneFrame *f = sg->frames[(aa_rx_frame_id)i_frame];
enum aa_rx_frame_type type = f->type;
bool update_abs = 0;
bool in_global = f->in_global();
const double *E_rel0 = TF_rel0 + i_rel0;
const double *E_abs0 = TF_abs0 + i_abs0;
double *E_rel = TF_rel + i_rel;
double *E_abs = TF_abs + i_abs;
switch( type ) {
case AA_RX_FRAME_FIXED:
f->tf_rel(q, E_rel);
update_abs = !in_global && updated[f->parent_id];
break;
case AA_RX_FRAME_REVOLUTE:
case AA_RX_FRAME_PRISMATIC: {
amino::SceneFrameJoint *fj = static_cast<amino::SceneFrameJoint*>(f);
if( aa_feq(q0[fj->config_index], q[fj->config_index], 0 )) {
AA_MEM_CPY(E_rel, E_rel0, 7);
update_abs = !in_global && updated[f->parent_id];
} else {
f->tf_rel(q, E_rel);
update_abs = 1;
}
break; }
}
if( update_abs ) {
// chain to global
if( in_global ) {
// TODO: can we somehow get rid of this branch?
// maybe a separate type for global frames
AA_MEM_CPY(E_abs, E_rel, 7);
} else {
assert( f->parent_id < (ssize_t)i_frame );
double *E_abs_parent = TF_abs + (ld_abs * f->parent_id);;
aa_tf_qutr_mul(E_abs_parent, E_rel, E_abs);
}
updated[i_frame] = 1;
} else {
AA_MEM_CPY(E_abs, E_abs0, 7);
updated[i_frame] = 0;
}
}
}
