void plot_qseg () {
aa_mem_region_t reg;
aa_mem_region_init( ®, 1024*64 );
double q0[6] = {0.296500, 0.000000, 0.178000, 0.000000, 3.141593, 0.000000};
double q1[6] = {0.380000, 0.003357, 0.089718, 0.482354, 3.045461, 0.989530};
double q2[6] = {1, 2, 3, 4, 5, 6};
size_t n_q = sizeof(q0)/sizeof(q0[0]);
struct rfx_trajq_points *points = rfx_trajq_points_alloc( ®, n_q );
rfx_trajq_points_add( points, 0, q0 );
rfx_trajq_points_add( points, 10, q1 );
rfx_trajq_points_add( points, 20, q2 );
//printf("n points: %lu\n", points->n_t );
//printf("points 0: %lx\n", points->point->head );
//printf("points 1: %lx\n", points->point->head->next );
struct rfx_trajq_seg_list *seglist = rfx_trajq_gen_pblend_tm1( ®, points, 1 );
rfx_trajq_seg_plot( seglist, .001 );
aa_mem_region_destroy( ® );
}
void local_region_destroy(void *r)
{
if( r ) {
aa_mem_region_t *reg = (aa_mem_region_t*)r;
aa_mem_region_destroy(reg);
free(reg);
}
}
void allocator() {
aa_mem_region_t reg;
aa_mem_region_init( ®, 1024 );
/*--- List ---*/
amino::RegionList<int>::allocator alloc(®);
amino::RegionList<int>::type list(alloc);
list.push_back(1);
list.push_back(2);
list.push_back(3);
printf("List:\n");
for( amino::RegionList<int>::iterator p = list.begin(); p != list.end(); p++ ) {
printf("> %d\n", *p);
}
/*--- Vector ---*/
amino::RegionVector<int>::allocator alloc1(®);
amino::RegionVector<int>::type vector(3, 0, alloc1);
vector[0] = 10;
vector[1] = 20;
vector[2] = 30;
printf("Vector:\n");
for( amino::RegionVector<int>::iterator p = vector.begin(); p != vector.end(); p++ ) {
printf("> %d\n", *p);
}
/*--- Map ---*/
amino::RegionMap<int,int>::allocator alloc2(®);
amino::RegionMap<int,int>::type map(std::less<int>(), alloc2);
map[1] = 10;
map[2] = 20;
map[3] = 30;
printf("Map:\n> %d %d %d\n", map[1], map[2], map[3] );
/*--- Stats ---*/
printf("Stats:\n"
"> start: 0x%lx\n"
"> head: 0x%lx\n"
"> used: %lu\n",
(intptr_t)reg.node->d,
(intptr_t)reg.head,
(intptr_t)reg.head - (intptr_t)reg.node->d
);
aa_mem_region_destroy( ® );
}
int better_example(void) {
// create a regeion memory allocator
aa_mem_region_t reg;
aa_mem_region_init( ®, 1024*32 );
// Create list for way points
struct rfx_trajx_point_list *plist = rfx_trajx_point_list_alloc( ® );
// waypoint translations
double X[5][5] = { {0,0,0}, {1,0,0}, {1,1,0}, {1,1,1}, {0,0,0} };
// waypoint euler angles
double E[5][5] = { {0,0,0}, {M_PI_2,0,0}, {M_PI_2,M_PI_2,0}, {M_PI_2,M_PI_2,M_PI_2}, {0,0,0} };
// storage for waypoint quaternions
double R[5][4];
// Add waypoints to point list
for( size_t i = 0; i < sizeof(X)/sizeof(X[0]); i ++ ) {
aa_tf_eulerzyx2quat( E[i][0], E[i][1], E[i][2], R[i] );
rfx_trajx_point_list_addb_qv( plist, 5*(double)i, 1, R[i], X[i] );
}
// generate trajectory
struct rfx_trajx_seg_list *seglist =
rfx_trajx_splend_generate( plist, ® );
// plot trajectory
//rfx_trajx_seg_list_plot( seglist, .001, NULL );
// print points
for( double t = 0; t < 10; t += .05 ) {
double T[12]; // column major [R | t]
double dx[6]; // xyz translational velocity, xyz rotational velocity
rfx_trajx_seg_list_get_dx_tfmat( seglist, t, T, dx );
printf("--\n");
aa_dump_vec( stdout, T, 12 );
aa_dump_vec( stdout, dx, 6 );
}
aa_mem_region_destroy( ® );
return 0;
}
void plot_viax() {
aa_mem_region_t reg;
aa_mem_region_init( ®, 1024*32 );
struct rfx_trajx_point_list *plist = rfx_trajx_point_list_alloc( ® );
double theta = M_PI*.9;
//double X[5][5] = { {0,0,0}, {1,0,0}, {1,1,0}, {1,1,1}, {0,0,0} };
//double E[5][5] = { {0,0,0}, {M_PI_2,0,0}, {M_PI_2,M_PI_2,0}, {M_PI_2,M_PI_2,M_PI_2}, {0,0,0} };
double X[2][3] = { {0,0,0}, {0,0,0} };
double E[2][3] = { {theta,0,0}, {theta,theta,0} };
double R[5][4];
double RV[5][3];
size_t n = 2;
for( size_t i = 0; i < n; i ++ ) {
aa_tf_eulerzyx2quat( E[i][0], E[i][1], E[i][2], R[i] );
aa_tf_quat2rotvec( R[i], RV[i] );
rfx_trajx_point_list_addb_qv( plist, 5*(double)i, 1, R[i], X[i] );
}
//rfx_trajx_generate( pT );
//rfx_trajx_plot( pT, .001, NULL );
struct rfx_trajx_seg_list *seglist =
//rfx_trajx_splend_generate( plist, ® );
rfx_trajx_parablend_generate( plist, ® );
rfx_trajx_seg_list_plot( seglist, .001, NULL );
return;
struct rfx_trajx_seg_list *testlist =
rfx_trajx_seg_list_alloc( ® );
{
double x_i[6], x_f[6];
AA_MEM_CPY(x_i, X[0], 3 );
AA_MEM_CPY(x_i+3, RV[0], 3 );
AA_MEM_CPY(x_f, X[1], 3 );
AA_MEM_CPY(x_f+3, RV[1], 3 );
struct rfx_trajx_seg *test =
rfx_trajx_seg_lerp_rv_alloc( ®, 0, 1,
0, x_i,
1, x_f ) ;
rfx_trajx_seg_list_add( testlist, test );
}
struct rfx_trajx_seg_list *testlist2 =
rfx_trajx_seg_list_alloc( ® );
{
struct rfx_trajx_seg *test =
rfx_trajx_seg_lerp_slerp_alloc( ®, 0, 1,
0, X[0], R[0],
1, X[1], R[1] ) ;
rfx_trajx_seg_list_add( testlist2, test );
}
rfx_trajx_seg_list_plot( testlist, .001, NULL );
aa_mem_region_destroy( ® );
}
~aa_ct_pt_list(void) {
list.~list();
aa_mem_region_destroy(®);
}
void aa_mem_pool_destroy( aa_mem_pool_t *pool ) {
aa_mem_region_destroy( &pool->region );
}
