24void threadsafe_enqueue_item(
s_list_t *item,
s_list_t** queue, pthread_mutex_t* queue_lock);
25s_list_t* threadsafe_dequeue_item(
s_list_t** queue, pthread_mutex_t *queue_lock );
26void threadsafe_enqueue_item_signal(
s_list_t *item,
s_list_t** queue, pthread_mutex_t* queue_lock, pthread_cond_t *queue_nonzero);
27s_list_t* threadsafe_dequeue_item_wait(
s_list_t** queue, pthread_mutex_t *queue_lock, pthread_cond_t *queue_nonzero, BOOL *waiting );
29int load_file_blob(
const char *filename,
char **blob,
int *len);
31void viewer_setpose(
double *quat4,
double *vec3);
32void viewer_getpose(
double *quat4,
double *vec3);
33void viewer_getbindpose(
double *quat4,
double *vec3);
39static pthread_mutex_t ssr_queue_mutex = PTHREAD_MUTEX_INITIALIZER;
40static pthread_cond_t ssr_queue_condition = PTHREAD_COND_INITIALIZER;
41static bool ssr_server_waiting = FALSE;
43void SSRserver_enqueue_request_and_wait(
void *fwctx,
void *ssr_request){
51 pthread_mutex_init(&request->requester_mutex,NULL);
52 pthread_cond_init(&request->requester_condition,NULL);
53 pthread_mutex_lock(&request->requester_mutex);
54 request->answered = 0;
57 if(1)threadsafe_enqueue_item(item,&ssr_queue, &ssr_queue_mutex);
58 if(0)threadsafe_enqueue_item_signal(item,&ssr_queue, &ssr_queue_mutex,&ssr_queue_condition);
59 while (request->answered == 0){
60 pthread_cond_wait(&request->requester_condition, &request->requester_mutex);
62 pthread_cond_destroy(&request->requester_condition);
63 pthread_mutex_destroy(&request->requester_mutex);
67#include "../lib/internal.h"
70#define GLDOUBLE double
72#include "../lib/scenegraph/quaternion.h"
73#include "../lib/scenegraph/LinearAlgebra.h"
74static double view[16], inv_view[16], matOri[16];
76static int vp2world_initialized = FALSE;
77static int use_vp2world = TRUE;
79static int reverse_sense_init = 0;
80static int reverse_sense_quat4 = 0;
81static int reverse_sense_vec3 = 0;
82static int reverse_order_quat4 = 1;
83void viewer_getview(
double *viewMatrix);
84void vp2world_initialize()
233 if(!vp2world_initialized){
236 double wholeview[16], mat[16], mat2[16], mat3[16], mat4[16], quat4[4], vec3[3], matidentity[16];
237 viewer_getview(wholeview);
238 printmatrix2(wholeview,
"view with .pos .ori");
242 loadIdentityMatrix(matidentity);
243 loadIdentityMatrix(mat);
244 viewer_getpose(quat4,vec3);
245 printf(
"fixing view, current pose vec3=%lf %lf %lf\n",vec3[0],vec3[1],vec3[2]);
249 double2quat(&qq,quat4);
250 quaternion_normalize(&qq);
251 loadIdentityMatrix(mat2);
252 quaternion_to_matrix(mat2, &qq);
253 printmatrix2(mat2,
"matqq using quaternion_to_matrix");
255 loadIdentityMatrix(mat3);
256 mattranslate(mat3,vec3[0],vec3[1],vec3[2]);
257 printmatrix2(mat3,
"mat3 vec3 translation");
259 double matinvpos[16],matinvquat[16];
260 matinverseAFFINE(matinvquat,mat2);
261 matinverseAFFINE(matinvpos,mat3);
264 matmultiply(mat,matinvquat,matinvpos);
265 printmatrix2(mat,
"inv(vec3) x inv(quat)");
273 matmultiply(view,wholeview,mat);
274 printmatrix2(view,
"view - no .pos .ori");
276 matinverseAFFINE(inv_view, view);
277 if(reverse_sense_init){
278 matrix_to_quaternion(&viewQuat, inv_view);
279 quaternion_normalize(&viewQuat);
280 quaternion_inverse(&inv_viewQuat,&viewQuat);
282 matrix_to_quaternion(&viewQuat, view);
283 quaternion_normalize(&viewQuat);
285 quaternion_inverse(&inv_viewQuat,&viewQuat);
287 matrix_to_quaternion(&inv_viewQuat,inv_view);
288 quaternion_normalize(&inv_viewQuat);
296 double matQtmp[16],matii[16];
297 viewer_getpose(quat4,vec3);
299 double2quat(&q4,quat4);
300 quaternion_normalize(&q4);
301 quaternion_multiply(&qtmp,&q4, &viewQuat);
302 quaternion_normalize(&qtmp);
304 loadIdentityMatrix(matQtmp);
305 quaternion_to_matrix(matQtmp,&qtmp);
306 printmatrix2(matQtmp,
"matQtmp - should look like view with pos, ori in 3x3");
307 matinverseAFFINE(matii,inv_view);
308 printmatrix2(matii,
"matii inv(inv(view)) should look like view no pos ori");
309 matinverseAFFINE(matii,wholeview);
310 printf(
"from inv(wholeview) avatar coords should match '/' command:\n [%lf %lf %lf]\n",matii[12],matii[13],matii[14]);
311 viewer_getpose(quat4,vec3);
312 vecnegated(vec3,vec3);
313 transformAFFINEd(vec3, vec3, inv_view);
314 printf(
"but does it match my theory of inv(view no pos/ori) x .Pos?:\n [%lf %lf %lf]\n",vec3[0],vec3[1],vec3[2]);
317 vp2world_initialized = TRUE;
321void SSR_reply_pose(
SSR_request *request,
int initialpose)
350 double quat4[4], vec3[3];
352 vp2world_initialize();
354 viewer_getbindpose(quat4,vec3);
357 viewer_getpose(quat4,vec3);
361 vecnegated(vec3,vec3);
362 transformAFFINEd(request->vec3, vec3, inv_view);
364 double2quat(&q4,quat4);
365 quaternion_normalize(&q4);
366 quaternion_multiply(&qtmp,&q4,&viewQuat);
367 quaternion_normalize(&qtmp);
368 quat2double(request->quat4,&qtmp);
370 memcpy(vec3,request->vec3,3*
sizeof(
double));
371 memcpy(quat4,request->quat4,4*
sizeof(
double));
377 viewer_getpose(request->quat4,request->vec3);
380static ssr_test_initialized = FALSE;
381static run_ssr_test = FALSE;
382char *get_key_val(
char *
key);
384static double incPitch;
385static double incTrans[3];
386static double incWtrans[3];
387static int haveInc = FALSE;
388void ssr_test_key_val(
char *
key,
char *val){
391 incTrans[0] = incTrans[1] = incTrans[2] = incYaw = incPitch = 0.0;
392 incWtrans[0] = incWtrans[1] = incWtrans[2] = 0.0;
394 ok = sscanf(val,
"%lf",&dval);
395 if(!strcmp(
key,
"yaw")){
398 if(!strcmp(
key,
"pitch")){
401 if(!strcmp(
key,
"x")){
404 if(!strcmp(
key,
"y")){
407 if(!strcmp(
key,
"z")){
410 if(!strcmp(
key,
"wx")){
413 if(!strcmp(
key,
"wy")){
416 if(!strcmp(
key,
"wz")){
426int ssr_test(
char *
keyval){
439 sep = strchr(kv,
',');
440 if(!sep) sep = strchr(kv,
' ');
446 ssr_test_key_val(
key,val);
454#define MATH_PI 3.14159265358979323846
456#ifndef DEGREES_PER_RADIAN
457#define DEGREES_PER_RADIAN (double)57.2957795130823208768
485void SSR_test_cumulative_pose(){
489 static int test_count = 0;
490 int test_full_cycle_here;
491 if(!ssr_test_initialized)
493 char *running_ssr = get_key_val(
"SSR");
496 if(!strcmp(running_ssr,
"true"))
497 run_ssr_test = FALSE;
498 ssr_test_initialized = TRUE;
500 if(!run_ssr_test)
return;
504 vp2world_initialized = FALSE;
505 vp2world_initialize();
506 test_full_cycle_here = FALSE;
507 if(test_full_cycle_here){
509 double matquat[16], matvec[16], mata[16], matcum[16], matb[16], matcuminv[16], matcumquat[16], matcumquatinv[16], matqb[16];
510 double quat4[4],vec3[3],quat4b[4],vec3b[3],zero3[3],cumpos[3],incTransb[3],vecpos[3];
511 Quaternion qa,cumquat,qb,cumquatinv, qa_inv, qc, incQuat, cumconj;
532 viewer_getpose(quat4,vec3);
533 zero3[0] = zero3[1] = zero3[2] = 0.0;
536 vecnegated(vec3,vec3);
537 transformAFFINEd(cumpos,vec3,inv_view);
538 double2quat(&qa,quat4);
539 quaternion_normalize(&qa);
540 quaternion_multiply(&cumquat,&qa,&viewQuat);
541 quaternion_normalize(&cumquat);
548 double ypr[3], axyz[3], dyaw, dpitch;
549 quaternion_print(&cumquat,
"cumquat before");
550 quaternion_inverse(&cumquatinv,&cumquat);
552 quat2yawpitch(ypr,&cumquatinv);
555 printf(
"1. yp =[%lf %lf]\n",rad2deg(dyaw),rad2deg(dpitch));
558 printf(
"2. yp =[%lf %lf]\n",rad2deg(dyaw),rad2deg(dpitch));
560 euler2quat(&cumquatinv,0.0,-dyaw,-dpitch);
561 quaternion_print(&cumquatinv,
"qi after euler2quat");
562 quaternion_inverse(&cumquat,&cumquatinv);
563 quaternion_print(&cumquat,
"cumquat after");
565 quaternion_rotationd(incTrans,&cumquatinv,incTrans);
566 vecaddd(cumpos,incTrans,cumpos);
568 vecaddd(cumpos,incWtrans,cumpos);
569 printf(
"cumpos [%lf %lf %lf]\n",cumpos[0],cumpos[1],cumpos[2]);
575 quaternion_multiply(&qb,&cumquat,&inv_viewQuat);
576 quaternion_normalize(&qb);
577 quat2double(quat4b,&qb);
579 transformAFFINEd(vec3b,cumpos,view);
580 vecnegated(vec3b,vec3b);
581 viewer_setpose(quat4b,vec3b);
588 SSR_reply_pose(&request,FALSE);
589 double2quat(&cumquat,request.quat4);
590 veccopyd(cumpos,request.vec3);
597 double ypr[3], axyz[3], dyaw, dpitch;
598 quaternion_print(&cumquat,
"cumquat before");
599 quaternion_inverse(&cumquatinv,&cumquat);
601 quat2yawpitch(ypr,&cumquatinv);
604 printf(
"1. yp =[%lf %lf]\n",rad2deg(dyaw),rad2deg(dpitch));
607 printf(
"2. yp =[%lf %lf]\n",rad2deg(dyaw),rad2deg(dpitch));
609 euler2quat(&cumquatinv,0.0,-dyaw,-dpitch);
610 quaternion_print(&cumquatinv,
"qi after euler2quat");
611 quaternion_inverse(&cumquat,&cumquatinv);
612 quaternion_print(&cumquat,
"cumquat after");
614 quaternion_rotationd(incTrans,&cumquatinv,incTrans);
615 vecaddd(cumpos,incTrans,cumpos);
617 vecaddd(cumpos,incWtrans,cumpos);
618 printf(
"cumpos [%lf %lf %lf]\n",cumpos[0],cumpos[1],cumpos[2]);
621 quat2double(request.quat4,&cumquat);
622 veccopyd(request.vec3,cumpos);
623 SSR_set_pose(&request);
639 double quat4[4], vec3[3];
641 vp2world_initialize();
642 transformAFFINEd(vec3,request->vec3,view);
643 vecnegated(vec3,vec3);
644 double2quat(&q4,request->quat4);
645 quaternion_multiply(&qtmp,&q4,&inv_viewQuat);
646 quaternion_normalize(&qtmp);
647 quat2double(quat4,&qtmp);
649 viewer_setpose(quat4,vec3);
652 viewer_setpose(request->quat4, request->vec3);
658void dequeue_SSR_request(
ttglobal tg)
673 int sleepincus = 1000;
674 int maxsleepus = 1000000;
676 while(!item && slept < maxsleepus) {
677 item = threadsafe_dequeue_item(&ssr_queue, &ssr_queue_mutex );
684 item = threadsafe_dequeue_item(&ssr_queue, &ssr_queue_mutex );
687 request = item->elem;
689 switch(request->type)
694 case SSR_POSESNAPSHOT:
695 SSR_set_pose(request);
701 ssr_current_request = request;
707static char *snapshot_filename =
"snapshot.bmp";
709static char *snapshot_filename =
"snapshot.png";
711void Snapshot1(
char *fname);
715 Snapshot1(snapshot_filename);
716 iret = load_file_blob(snapshot_filename,&request->blob,&request->len);
718 printf(
"snapshot file not found %s\n",snapshot_filename);
720 unlink(snapshot_filename);
726 if(ssr_current_request){
728 switch(request->type){
730 SSR_reply_pose(request,TRUE);
break;
732 SSR_reply_pose(request,FALSE);
break;
733 case SSR_POSESNAPSHOT:
734 SSR_reply_snapshot(request);
break;
739 request->answered = 1;
740 pthread_cond_signal(&request->requester_condition);
741 ssr_current_request = NULL;