FreeWRL/FreeX3D  3.0.0
SSRhelper.c
1 #include <config.h>
2 #ifdef SSR_SERVER
3 /*
4  D. SSR: server-side rendering
5  SSRhelper: supplies the libfreewrl parts:
6  a) a queue for SSR client requests:
7  i) pose-pose - given a viewpoint pose, run a draw() including collision,
8  animation, to compute a new pose, and return the new pose
9  ii) pose-snapshot - given a viewpoint pose, run a draw() and render
10  a frame, do a snapshot, convert snapshot to .jpg and return .jpg as blob (char* binary large object)
11  b) a function for _DisplayThread to run once per frame
12  What's not in here:
13  A. the html client part - talks to the web server part
14  B. the web server part - talks to the html client part, and to this SSRhelper part in libfreewrl
15  C. libfreewrl (dllfreewrl) export interfaces on the enqueue function
16 */
17 #include <stdio.h>
18 #include <stdlib.h>
19 #include <string.h>
20 #include <system.h>
21 #include "list.h"
22 #include "SSRhelper.h"
23 //from Prodcon.c L.1100
24 void threadsafe_enqueue_item(s_list_t *item, s_list_t** queue, pthread_mutex_t* queue_lock);
25 s_list_t* threadsafe_dequeue_item(s_list_t** queue, pthread_mutex_t *queue_lock );
26 void threadsafe_enqueue_item_signal(s_list_t *item, s_list_t** queue, pthread_mutex_t* queue_lock, pthread_cond_t *queue_nonzero);
27 s_list_t* threadsafe_dequeue_item_wait(s_list_t** queue, pthread_mutex_t *queue_lock, pthread_cond_t *queue_nonzero, bool *waiting );
28 //from io_files.c L.310
29 int load_file_blob(const char *filename, char **blob, int *len);
30 //from Viewer.c L.1978
31 void viewer_setpose(double *quat4, double *vec3);
32 void viewer_getpose(double *quat4, double *vec3);
33 void viewer_getbindpose(double *quat4, double *vec3);
34 
35 #define FALSE 0
36 #define TRUE 1
37 typedef struct iiglobal *ttglobal;
38 static s_list_t *ssr_queue = NULL;
39 static pthread_mutex_t ssr_queue_mutex = PTHREAD_MUTEX_INITIALIZER;
40 static pthread_cond_t ssr_queue_condition = PTHREAD_COND_INITIALIZER;
41 static bool ssr_server_waiting = FALSE;
42 
43 void SSRserver_enqueue_request_and_wait(void *fwctx, SSR_request *request){
44  //called by A -> B -> C
45  //in B -the web server- a new thread is created for each A request.
46  //this function is called from those many different temporary threads
47  //the backend _displayThread will own our queue -so it's a thread funnel
48  s_list_t *item = ml_new(request);
49  pthread_mutex_init(&request->requester_mutex,NULL);
50  pthread_cond_init(&request->requester_condition,NULL);
51  pthread_mutex_lock(&request->requester_mutex);
52  request->answered = 0;
53  request->blob = NULL;
54  request->len = 0;
55  if(1)threadsafe_enqueue_item(item,&ssr_queue, &ssr_queue_mutex);
56  if(0)threadsafe_enqueue_item_signal(item,&ssr_queue, &ssr_queue_mutex,&ssr_queue_condition);
57  while (request->answered == 0){
58  pthread_cond_wait(&request->requester_condition, &request->requester_mutex);
59  }
60  pthread_cond_destroy(&request->requester_condition);
61  pthread_mutex_destroy(&request->requester_mutex);
62 
63  return;
64 }
65 #include "../lib/internal.h"
66 #define BOOL int
67 #ifndef GLDOUBLE
68 #define GLDOUBLE double
69 #endif
70 #include "../lib/scenegraph/quaternion.h"
71 #include "../lib/scenegraph/LinearAlgebra.h"
72 static double view[16], inv_view[16], matOri[16];
73 static Quaternion viewQuat, inv_viewQuat;
74 static int vp2world_initialized = FALSE;
75 static int use_vp2world = TRUE;
76 #include <math.h>
77 static int reverse_sense_init = 0; //0 conceptually correct
78 static int reverse_sense_quat4 = 0; //0 conceptually correct
79 static int reverse_sense_vec3 = 0;
80 static int reverse_order_quat4 = 1; //0 conceptually correct
81 void viewer_getview( double *viewMatrix);
82 void vp2world_initialize()
83 {
84  /*
85  Computes the View' and inverseView' transforms where View' is View except without the .Quat, .Pos effects
86  View' = inv(.Quat) x inv(.Pos) x View
87  - then SSR_reply_pose() and SSR_set_pose() can transform client-side quat4,vec3 to/from viewer.Quat,.Pos:
88  (vec3,quat4) = (.Pos,.Quat) x View'
89  (.Pos,.Quat) = (vec3,quat4) x invView'
90  For SSR, assume View doesn't change through life of scene:
91  - no viewpoint animation
92  - no switching bound viewpionts
93  -> then we only call this once on init ie ssr's init_pose (it's a bit heavy with inverses)
94  and assume this call is being made from outside render_hier()
95  - after seeking the view matrix
96  - so just the view matrix is in opengl's modelview matrix
97 
98  Colum-major vs Row-Major notation - it's a notion in the mind, and either way is correct
99  https://www.opengl.org/archives/resources/faq/technical/transformations.htm
100  opengl matrices elements 12,13,14 are the translations;
101  3,7,11 are perspectives (zero for modelview, nonzero for projection); 15 == 1
102  OpenGL manuals show column major which is a bit counter-intuitive for C programmers:
103  [x] [ 0 4 8 12] [x]
104  [y] = [ 1 5 9 13] x [y] Column major notation, as OGL manuals show
105  [z] [ 2 6 10 14] [z] 4x1 = 4x4 x 4x1
106  [w] [ 3 7 11 15] [1]
107  Equivalent C row major order:
108  [x y z w] = [x y z 1] x [ 0 1 2 3]
109  . [ 4 5 6 7] Row major notation
110  . [ 8 9 10 11] 1x4 = 1x4 x 4x4
111  . [12 13 14 15]
112  Note the notational order on right hand side is opposite. Below we will use the opengl column major ordering
113  transform(r,a,M): r = M x [a,1] //note vector in column form on right of matrix
114  matmultiply(r,a,b): r = a x b
115  (client gl-matrix.js is in opengl matrix order, same transform, but reverses order in matrix multiply)
116 
117  The sense/direction of the view is the opengl sense of modelview: transform world2vp.
118 
119  vp2world sense:
120  world - View - Viewpoint - .position - .orientation - vp
121  world2vp sense (what is stored in opengl modelview matrix):
122  [vp xyz] = world2vpMatrix x [world xyz] = ViewMatrix x [world]
123  [vp xyz] = viewMatrix x modelMatrix x [shape xyz]
124  ViewMatrix in world2vp sense (and opengl row-major order):
125  ViewMatrix = viewer.Quat x -viewer.Pos x viewer.AntiPos x viewer.AntiQuat x -vp.orientation x -vp.position x -transforms
126  View (world2vp sense, row major order) = (mobile device screen orientation) x world2vp(.Quat, .Pos) x vp2world(.AntiPos, .AntiQuat) x world2vp(.orientation, .position) x Transforms(in vp2world direction)
127  assuming no screen orientation, and (.AntiPos,.AntiQuat) == (cancels) .orientation,.position, this simiplfies to:
128  View (world2vp sense, row major order) = x (.Quat in world2vp, -(.Pos in vp2world)) x Transforms(in world2vp sense)
129 
130  opengl sense:
131  (a glTranslate(0,0,-5) will translate an object in world coordinates by -5 along camera/viewpoint Z axis.
132  Viewpoint/camera Z axis +backward, so -5 on object shifts the object 5 further away from camera
133  assuming it starts in front of the camera. In this interpretation, object at world 0,0,0 gets transformed to
134  camera/viewpoint 0,0,-5, and modelview matrix is in the sense of world to camera, or world2vp)
135  a glRotate(+angle,1,0,0) will rotate counter clockwise about the axis as seen looking down the axis toward the origin
136  -or what is normally called 'right hand rule': align the thumb on your right hand with the axis, and the
137  way the fingers curl is the direction of rotation
138  -rotates world2vp
139 
140  x3d sense:
141  X3D's transform stack between root/world and viewpoint -the View matrix equivalent-
142  is declared in X3D in the sense of (leaf object) to world or in our case (viewpoint) to world, or vp2world
143  X3D.s viewpoint .position, .orientation is also declared in the vp2world sense
144  freewrl reverses the x3d sense when building the View part of the Modelview, to put it in the opengl world2vp sense
145  - see INITIATE_POSITION_ANTIPOSITION macro, as used in bind_viewpoint()
146  - see viewpoint_togl() as called from setup_viewpoint()
147  - see fin_transform() for render_vp/VF_Viewpoint pass
148 
149  For example a translation of 10,0,0 in a Transform around bound vp will translate vp to the right in the world,
150  that's a vp2world sense
151  A vp.position=10,0,0 has the same sense and magnitude, vp2world
152  To get this effect in an opengl modelview matrix transforming world2vp, freewrl subtracts 10 / uses -10 when
153  creating the opengl matrix.
154  And when converting .orientation to Quat, it inverts the direction.
155  And when using .Pos = .position, it negates ie glTranslate(-position.x,-position.y,-position.z) and -.Pos.x, -Pos.y, -Pos.z
156 
157  Note on quaternion commutivity: qa * qb != qb * qa -in general 3D rotations are non-commutative.
158  If you want to reverse the order of matrices C = A*B to get equivalent C = B'*A, you compute:
159  B'*A = A*B
160  multiply each side by inv(A)
161  inv(A)*B'A = inv(A)*A*B = Identity*B = B
162  rearranging:
163  B = inv(A)*B'*A
164  post-multiply each side by inv(A)
165  B*inv(A) = inv(A)*B'*A*inv(A) = inv(A)*B'*Identity = inv(A)*B'
166  multiply each side by A
167  A*B*inv(A) = A*inv(A)*B' = Identity*B' = B'
168  changing sides
169  B' = A*B*inv(A)
170  check by substituting in C=B'*A
171  C = A*B*inv(A)*A = A*B*Identity = A*B
172  If you want to reverse the order of quaternions qc = qa * qb = qb' * qa,
173  qb' * qa = qa * qb
174  qb' * qa * conj(qa) = qa * qb * conj(qa)
175  qb' = Identity = qa * qb * conj(qa)
176  qb' = qa * qb * conj(qa) //method A
177  Numerically you can test, comparing to textbook formulas
178  qa*qb = conj(conj(qb)*conj(qa)) //method B
179  == qb'*qa ?
180  qb'*qa = conj(conj(qb)*conj(qa))
181  qb'*qa*conj(qa) = conj(conj(qb)*conj(qa))*conj(qa)
182  qb' = conj(conj(qb)*conj(qa))*conj(qa) ?
183 
184 
185  conceptually correct transforms, in row-major-notation order:
186  client (html javascript using gl-matrix.js):
187  viewpoint < cumQuat < cumPos < worldGrid
188  viewpoint = cumQuat x cumPos x [worldGrid]
189  server (freewrl):
190  viewpoint < .Quat < -.Pos < View' < world
191  viewpoint = .Quat x -.Pos x View' x [world]
192  All the above transforms cumQuat,cumPos, .Quat, .Pos, View conceptually should have the world2vp sense '<'
193  To reverse the sense of the equation from vp2world to world2vp we multiply each side by the inverse of the
194  last-applied/first-notational transform on the right side, then inverse-something x something = Identity/I/1.0
195  invQuat x viewpoint = invQuat x Quat x -Pos x View' x [world]
196  = Identity x -Pos x View' x [world]
197  = -Pos x View' x [world]
198  inv(-Pos) x invQuat x viewpoint = inv(-Pos) x -Pos x View' x [world]
199  = View' x [world]
200  invView' x inv(-Pos) x invQuat x viewpoint = invView' x View' x [world] = [world]
201  changing sides:
202  [world] = invView' x inv(-Pos) x invQuat x viewpoint
203 
204  Details:
205  server_side
206  A.vp2world_initialize
207  1) view = modelview at top of glmatrix stack, at scene root, in setup_viewpoint()
208  after viewer_togl() and render_heir() VF_Viewpoint pass
209  - includes .Quat, -.Pos
210  2) a) view' = f(view .Quat, .Pos)
211  b) view' = inv(-Pos) x inv(.Quat) x View
212  = inv(Quat x (-Pos)) x View
213  3) viewQuat' = matrix_to_quaternion(view')
214  4) invView = inv(view'), invQuat = inv(viewQuat') //converts from world2vp sense to vp2world sense
215  B.reply_pose
216  1) cumPos = f(view',.Quat,.Pos) = invView x -.Pos //we want .pos to be more 'worldly'
217  2) cumQuat = f(view',.Quat,.Pos) = ViewQuat x .Quat //we just want to concatonate 2 quats, keeping the same sense
218  C.set_pose
219  1) .Pos = f(view',cumQuat,cumPos)
220  mulitplying each side of B1 by view':
221  view' x cumPos = view' x invView' x -.Pos
222  = Identity x -.Pos = -.Pos
223  .Pos = view' x -cumPos
224  2) .Quat = f(view',cumQuat,cumPos)
225  mulitplying each side of B2 by inv(viewQuat):
226  inv(viewQuat) x cumQuat = inv(viewQuat) x viewQuat x .Quat
227  = Identity x .Quat = .Quat
228  .Quat = inv(viewQuat) x cumQuat
229 
230  */
231  if(!vp2world_initialized){
232  int test_init;
233  //world2vp sense of view
234  double wholeview[16], mat[16], mat2[16], mat3[16], mat4[16], quat4[4], vec3[3], matidentity[16];
235  viewer_getview(wholeview); //gets modelview matrix and assumes it is ViewMatrix, and in opengl sense world2vp
236  printmatrix2(wholeview,"view with .pos .ori");
237  //problem: view matrix also has world2vp(.position, .orientation) in it. We'd like to keep them separate.
238  //solution: construct a few matrices with vec3, quat4, invert, and multiply view by them
239  //View' = View x inverse(.Quat,-.Pos) //world2vp
240  loadIdentityMatrix(matidentity);
241  loadIdentityMatrix(mat);
242  viewer_getpose(quat4,vec3); //viewer.position,.orientation, in sense of world2vp (vec3 = -.Pos, .Pos = .position, quat4 = .Quat = inv(.orientation)
243  printf("fixing view, current pose vec3=%lf %lf %lf\n",vec3[0],vec3[1],vec3[2]);
244 
245  {
246  Quaternion qq;
247  double2quat(&qq,quat4);
248  quaternion_normalize(&qq);
249  loadIdentityMatrix(mat2);
250  quaternion_to_matrix(mat2, &qq);
251  printmatrix2(mat2,"matqq using quaternion_to_matrix");
252  }
253  loadIdentityMatrix(mat3);
254  mattranslate(mat3,vec3[0],vec3[1],vec3[2]);
255  printmatrix2(mat3,"mat3 vec3 translation");
256  {
257  double matinvpos[16],matinvquat[16];
258  matinverseAFFINE(matinvquat,mat2);
259  matinverseAFFINE(matinvpos,mat3);
260  // view' x pos x invpos = ivew x invquat x invpos
261  // view' = view x invquat x invpos = view x inv(pos x quat)
262  matmultiply(mat,matinvquat,matinvpos); //RIGHT
263  printmatrix2(mat,"inv(vec3) x inv(quat)");
264  }
265 
266  /* view' x pos x quat = view
267  view' x pos x quat x invquat = view x invquat
268  view' x pos x invpos = ivew x invquat x invpos
269  view' = view x invquat x invpos = view x inv(pos x quat)
270  */
271  matmultiply(view,wholeview,mat); //RIGHT
272  printmatrix2(view,"view - no .pos .ori");
273  //now view should not have the .position, .orientation in it - just the transform stack from world to vp
274  matinverseAFFINE(inv_view, view); //we'll prepare inverse so we can transform position both ways
275  if(reverse_sense_init){
276  matrix_to_quaternion(&viewQuat, inv_view);
277  quaternion_normalize(&viewQuat);
278  quaternion_inverse(&inv_viewQuat,&viewQuat); //prepare inverse in a way we can also transform .orientation in the form of a quaternion
279  }else{
280  matrix_to_quaternion(&viewQuat, view); //conceptually correct
281  quaternion_normalize(&viewQuat);
282  if(0){
283  quaternion_inverse(&inv_viewQuat,&viewQuat); //prepare inverse in a way we can also transform .orientation in the form of a quaternion
284  }else{
285  matrix_to_quaternion(&inv_viewQuat,inv_view); //s.b. equavalent
286  quaternion_normalize(&inv_viewQuat);
287  }
288  }
289  test_init = TRUE;
290  if(test_init){
291  //verify, by computing total quat as we do below in get_pose, and verifying
292  //. cumQuat -> matrix matches original ivew with pos/ori
293  Quaternion q4,qtmp;
294  double matQtmp[16],matii[16];
295  viewer_getpose(quat4,vec3);
296  //printf("getting current viewpoint pose:\n");
297  double2quat(&q4,quat4);
298  quaternion_normalize(&q4);
299  quaternion_multiply(&qtmp,&q4, &viewQuat); //RIGHT
300  quaternion_normalize(&qtmp);
301 
302  loadIdentityMatrix(matQtmp);
303  quaternion_to_matrix(matQtmp,&qtmp);
304  printmatrix2(matQtmp,"matQtmp - should look like view with pos, ori in 3x3");
305  matinverseAFFINE(matii,inv_view);
306  printmatrix2(matii,"matii inv(inv(view)) should look like view no pos ori");
307  matinverseAFFINE(matii,wholeview);
308  printf("from inv(wholeview) avatar coords should match '/' command:\n [%lf %lf %lf]\n",matii[12],matii[13],matii[14]);
309  viewer_getpose(quat4,vec3);
310  vecnegated(vec3,vec3);
311  transformAFFINEd(vec3, vec3, inv_view);
312  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]);
313 
314  }
315  vp2world_initialized = TRUE;
316  }
317 }
318 //struct point_XYZ {GLDOUBLE x,y,z;};
319 void SSR_reply_pose(SSR_request *request, int initialpose)
320 {
321  /* client's pose(vec3,quat4) - world - View - (Viewpoint node) - .Pos - ..Quat - vp
322  Thinking of vec3,quat4 as things to transform:
323  transform server's scene bound-viewpoint-relative pose(Viewer.Pos,.Quat) to client's world coords pose(vec3,quat4)
324  Or thinking of vec3,quat4 as part of a transform chain:
325  add on View part of (quat4,vec3) transform to get from world2vp
326  (except we don't need any View scale on the client - it has its own scale for moving around,
327  so the 'vp as a point to be transformed' way of thinking above makes more sense)
328  Assume View doesn't change through life of scene:
329  - no viewpoint animation
330  - no switching bound viewpionts
331  initialpose - TRUE: for init_pose() reutrn the bind-time .pos,.quat
332  - FALSE: for posepose, return adjusted pose sent with posepose request
333  */
334  if(use_vp2world){
335  //convert boundviewpoint2world
336  // viewpoint-local .pos, .quat to world (scene root) coordinate system by applying inv(ViewMatrix)
337  // the .position/.Pos is on the world side of the .orientation/quat
338  // view matrix can be thought of as (Rot) x (translation)
339  // if you want to change the order to translation' x Rot,
340  // remember rotations and translations are not commutative. Then:
341  // translation' = invrse(Rot) x translation
342  // then View = (translation') x (Rot)
343  // vp = Quat x Pos x ViewTrans x ViewRot x world
344  // to gather the translations together, so we have
345  // vp = TotalQuat x TotalVec x world
346  // TotalQuat = Quat x ViewRot //3D rotations are not commutative, so their order needs to be maintained
347  // TotalVec = inverse(ViewRot) x (Pos + ViewTrans)
348  double quat4[4], vec3[3];
349  Quaternion qtmp, q4;
350  vp2world_initialize();
351  if(initialpose){
352  viewer_getbindpose(quat4,vec3);
353  //printf("getting initial viewpoint pose:\n");
354  }else{
355  viewer_getpose(quat4,vec3);
356  //printf("getting current viewpoint pose:\n");
357  }
358 
359  vecnegated(vec3,vec3);
360  transformAFFINEd(request->vec3, vec3, inv_view);
361 
362  double2quat(&q4,quat4);
363  quaternion_normalize(&q4);
364  quaternion_multiply(&qtmp,&q4,&viewQuat);
365  quaternion_normalize(&qtmp);
366  quat2double(request->quat4,&qtmp);
367 
368  memcpy(vec3,request->vec3,3*sizeof(double));
369  memcpy(quat4,request->quat4,4*sizeof(double));
370  //printf("getting server pose quat4=[%lf %lf %lf %lf] vec3=[%lf %lf %lf]\n",
371  //quat4[0],quat4[1],quat4[2],quat4[3],vec3[0],vec3[1],vec3[2]);
372  }else{
373  //assume View is identity/at scene root/no bound viewpoint
374  // send bound viewpoint relative .position, .orientaiton
375  viewer_getpose(request->quat4,request->vec3);
376  }
377 }
378 static ssr_test_initialized = FALSE;
379 static run_ssr_test = FALSE;
380 char *get_key_val(char *key);
381 static double incYaw;
382 static double incPitch;
383 static double incTrans[3];
384 static double incWtrans[3];
385 static int haveInc = FALSE;
386 void ssr_test_key_val(char *key, char *val){
387  double dval;
388  int ok;
389  incTrans[0] = incTrans[1] = incTrans[2] = incYaw = incPitch = 0.0;
390  incWtrans[0] = incWtrans[1] = incWtrans[2] = 0.0;
391 
392  ok = sscanf(val,"%lf",&dval);
393  if(!strcmp(key,"yaw")){
394  incYaw = dval;
395  } else
396  if(!strcmp(key,"pitch")){
397  incPitch = dval;
398  } else
399  if(!strcmp(key,"x")){
400  incTrans[0] = dval;
401  } else
402  if(!strcmp(key,"y")){
403  incTrans[1] = dval;
404  } else
405  if(!strcmp(key,"z")){
406  incTrans[2] = dval;
407  } else
408  if(!strcmp(key,"wx")){
409  incWtrans[0] = dval;
410  } else
411  if(!strcmp(key,"wy")){
412  incWtrans[1] = dval;
413  } else
414  if(!strcmp(key,"wz")){
415  incWtrans[2] = dval;
416  }
417 
418  haveInc = TRUE;
419 }
420 //#include <stdio.h>
421 //#include <string.h>
422 
423 
424 int ssr_test(char *keyval){
425  //save arbitrary char* keyval = "key,val" pairs,
426  // for later retrieval with print_keyval or get_key_val
427  int i, iret;
428  char kv[100];
429  i = strlen(keyval);
430  iret = 0;
431  if(i > 100)
432  iret = -1;
433  else
434  {
435  char *sep;
436  strcpy(kv,keyval);
437  sep = strchr(kv,',');
438  if(!sep) sep = strchr(kv,' ');
439  if(sep){
440  char *key, *val;
441  val = &sep[1];
442  (*sep) = '\0';
443  key = kv;
444  ssr_test_key_val(key,val);
445  iret = 1;
446  }
447  }
448  return iret;
449 }
450 void SSR_set_pose(SSR_request *request);
451 #ifndef MATH_PI
452 #define MATH_PI 3.14159265358979323846
453 #endif
454 #ifndef DEGREES_PER_RADIAN
455 #define DEGREES_PER_RADIAN (double)57.2957795130823208768
456 #endif
457 void test_euler();
458 /*
459 void quat2yawpitch(){
460  var iq = quat.create();
461  quat.invert(iq,cumQuat);
462  var ypr = vec3.create();
463  quat2yawpitch0(ypr,iq);
464  dyaw = -ypr[0];
465  dpitch = -ypr[1];
466  //console.log("in q2e yaw, pitch= "+rad2deg(dyaw)+" "+rad2deg(dpitch));
467 }
468 void yawpitch2quat(){
469  var qpitch = quat.create();
470  var qyaw = quat.create();
471  var qyp = quat.create();
472  quat.identity(qpitch);
473  quat.rotateX(qpitch,qpitch,dpitch);
474  //console.log("in yawpitch2quat dpitch="+dpitch);
475  quat.rotateY(qyaw,qyaw,dyaw);
476  //kinda works
477  var qi = quat.create();
478  //console.log("in e2q dyaw, dpitch ="+rad2deg(dyaw)+" "+rad2deg(dpitch));
479  euler2quat(qi,0.0,-dyaw,-dpitch);
480  quat.invert(cumQuat,qi);
481 }
482 */
483 void SSR_test_cumulative_pose(){
484  SSR_request r;
485  //we don't want to run this test when doing SSR, just when running normal freewrl.
486  //Its a kind of SSR client emulator test.
487  static int test_count = 0;
488  int test_full_cycle_here; //full cycle right here (else test SSR_set_pose(), SSR_reply_pose() )
489  if(!ssr_test_initialized)
490  {
491  char *running_ssr = get_key_val("SSR");
492  run_ssr_test = TRUE; // <<<< turn on/off test for freewrl here (must #define SSR_SERVER to get in here)
493  if(running_ssr)
494  if(!strcmp(running_ssr,"true"))
495  run_ssr_test = FALSE;
496  ssr_test_initialized = TRUE;
497  }
498  if(!run_ssr_test) return;
499 
500  test_count ++;
501  if(test_count < 100)
502  vp2world_initialized = FALSE;
503  vp2world_initialize();
504  test_full_cycle_here = FALSE;
505  if(test_full_cycle_here){
506  //does full cycle math right here
507  double matquat[16], matvec[16], mata[16], matcum[16], matb[16], matcuminv[16], matcumquat[16], matcumquatinv[16], matqb[16];
508  double quat4[4],vec3[3],quat4b[4],vec3b[3],zero3[3],cumpos[3],incTransb[3],vecpos[3];
509  Quaternion qa,cumquat,qb,cumquatinv, qa_inv, qc, incQuat, cumconj;
510 
511  /*
512  server-side
513  vp = world x view x pos x quat
514  client-side
515  vp = world x vec3 x quat4
516  client == server
517  world x vec3 x quat4 = world x view x pos x quat
518  vec3 x quat4 = view x pos x quat
519  inv_view x vec3 x quat4 = inv_view x view x pos x quat = pos x quat
520  solve rotations independenty first, because rotations are associative?
521  quat4 = view x quat
522  quat = inv_view x quat4
523  then solve vector?
524  vec3 x quat4 x invquat4 = view x pos x quat x invquat4
525  vec3 = view x pos x quat x invquat4
526  inv_view x vec3 x quat4 = pos x quat
527  inv_view x vec3 x quat4 x inv_quat = pos x quat x inv_quat = pos
528  pos = inv_view x vec3 x quat4 x inv_quat
529  */
530  viewer_getpose(quat4,vec3);
531  zero3[0] = zero3[1] = zero3[2] = 0.0; //vp in vp space = 0,0,0
532 
533  //goal: get incWtrans world coord +- working, without breaking inctrans
534  vecnegated(vec3,vec3);
535  transformAFFINEd(cumpos,vec3,inv_view);
536  double2quat(&qa,quat4);
537  quaternion_normalize(&qa);
538  quaternion_multiply(&cumquat,&qa,&viewQuat); //cumquat should be in world2vp sense like view
539  quaternion_normalize(&cumquat);
540 
541  //Step 2 add on global increments like SSRClient.html does
542  if(haveInc){
543  //client assumes world Z is up, and yaw is around world Z axis
544  // and pitch is relative to world horizon plane XY
545  //quat2yawpitch
546  double ypr[3], axyz[3], dyaw, dpitch;
547  quaternion_print(&cumquat,"cumquat before");
548  quaternion_inverse(&cumquatinv,&cumquat);
549 
550  quat2yawpitch(ypr,&cumquatinv);
551  dyaw = -ypr[0];
552  dpitch = -ypr[1];
553  printf("1. yp =[%lf %lf]\n",rad2deg(dyaw),rad2deg(dpitch));
554  dyaw += incYaw;
555  dpitch += incPitch;
556  printf("2. yp =[%lf %lf]\n",rad2deg(dyaw),rad2deg(dpitch));
557 
558  euler2quat(&cumquatinv,0.0,-dyaw,-dpitch);
559  quaternion_print(&cumquatinv,"qi after euler2quat");
560  quaternion_inverse(&cumquat,&cumquatinv);
561  quaternion_print(&cumquat,"cumquat after");
562  //transform incTrans from vp to world
563  quaternion_rotationd(incTrans,&cumquatinv,incTrans);
564  vecaddd(cumpos,incTrans,cumpos);
565  //incWtrans should be in world coords
566  vecaddd(cumpos,incWtrans,cumpos);
567  printf("cumpos [%lf %lf %lf]\n",cumpos[0],cumpos[1],cumpos[2]);
568  haveInc = FALSE;
569  }
570 
571  //Step 3 convert back to quat, pos
572  //quat = inv_view x quat4
573  quaternion_multiply(&qb,&cumquat,&inv_viewQuat);
574  quaternion_normalize(&qb);
575  quat2double(quat4b,&qb);
576  //pos = inv_view x vec3 x quat4 x inv_quat
577  transformAFFINEd(vec3b,cumpos,view);
578  vecnegated(vec3b,vec3b);
579  viewer_setpose(quat4b,vec3b);
580  } else {
581  //tests SSR_set_pose(), SSR_reply_pose() and does just the haveInc here
582  SSR_request request;
583  double cumpos[3];
584  Quaternion cumquat, incQuat,cumquatinv;
585 
586  SSR_reply_pose(&request,FALSE);
587  double2quat(&cumquat,request.quat4);
588  veccopyd(cumpos,request.vec3);
589  //Step 2 add on global increments like SSRClient.html does
590  //(an increment being a navigation-caused increment to x,y,z,yaw in vp space, or wx,wy,wz in world space)
591  if(haveInc){
592  //client assumes world Z is up, and yaw is around world Z axis
593  // and pitch is relative to world horizon plane XY
594  //quat2yawpitch
595  double ypr[3], axyz[3], dyaw, dpitch;
596  quaternion_print(&cumquat,"cumquat before");
597  quaternion_inverse(&cumquatinv,&cumquat);
598 
599  quat2yawpitch(ypr,&cumquatinv);
600  dyaw = -ypr[0];
601  dpitch = -ypr[1];
602  printf("1. yp =[%lf %lf]\n",rad2deg(dyaw),rad2deg(dpitch));
603  dyaw += incYaw;
604  dpitch += incPitch;
605  printf("2. yp =[%lf %lf]\n",rad2deg(dyaw),rad2deg(dpitch));
606 
607  euler2quat(&cumquatinv,0.0,-dyaw,-dpitch);
608  quaternion_print(&cumquatinv,"qi after euler2quat");
609  quaternion_inverse(&cumquat,&cumquatinv);
610  quaternion_print(&cumquat,"cumquat after");
611  //transform incTrans from vp to world
612  quaternion_rotationd(incTrans,&cumquatinv,incTrans);
613  vecaddd(cumpos,incTrans,cumpos);
614  //incWtrans should be in world coords
615  vecaddd(cumpos,incWtrans,cumpos);
616  printf("cumpos [%lf %lf %lf]\n",cumpos[0],cumpos[1],cumpos[2]);
617  haveInc = FALSE;
618  }
619  quat2double(request.quat4,&cumquat);
620  veccopyd(request.vec3,cumpos);
621  SSR_set_pose(&request);
622  }
623 }
624 
625 void SSR_set_pose(SSR_request *request)
626 {
627  /* request->quat4,vec3 - world - View - (Viewpoint node) - .Pos - .Quat - vp
628  tranform client's pose from its world coordinates to scene's bound-viewpoint-relative vp coords
629  Assume View doesn't change through life of scene:
630  - no viewpoint animation
631  - no switching bound viewpionts
632  */
633  if(use_vp2world){
634  //convert world2boundviewpoint / world2vp
635  // .pos, .quat world (scene root) coordinate system to bound viewpoint relative
636  // by applying ViewMatrix
637  double quat4[4], vec3[3];
638  Quaternion qtmp, q4;
639  vp2world_initialize();
640  transformAFFINEd(vec3,request->vec3,view);
641  vecnegated(vec3,vec3);
642  double2quat(&q4,request->quat4);
643  quaternion_multiply(&qtmp,&q4,&inv_viewQuat); //like test
644  quaternion_normalize(&qtmp);
645  quat2double(quat4,&qtmp);
646  //printf("setting server pose quat4=[%lf %lf %lf %lf] vec3=[%lf %lf %lf]\n",quat4[0],quat4[1],quat4[2],quat4[3],vec3[0],vec3[1],vec3[2]);
647  viewer_setpose(quat4,vec3);
648  }else{
649  //assume View is identity / at scene root / no bound viewpoint
650  viewer_setpose(request->quat4, request->vec3);
651  }
652 }
653 
654 static SSR_request *ssr_current_request = NULL; //held for one draw loop
655 int isSceneLoaded();
656 void dequeue_SSR_request(ttglobal tg)
657 {
658  //called by D: _DisplayThread, should be in backend thread, gglobal() should work
659  s_list_t *item;
660  SSR_request *request = NULL;
661  item = NULL;
662  if(isSceneLoaded()){
663  //item = threadsafe_dequeue_item_wait(&ssr_queue, &ssr_queue_mutex,&ssr_queue_condition,&ssr_server_waiting);
664  //item = threadsafe_dequeue_item_timed_wait(&ssr_queue, &ssr_queue_mutex,&ssr_queue_condition,&ssr_server_waiting);
665  //pthread timed wait seems complicated, so I'll use usleep in a loop - laziness
666  //Goal: allow a bit of server work to continue (not frozen)
667  // while freeing CPU cores and GPU from senseless drawing,
668  // allowing many/100s of SSR process instances to be running on the same server.
669  // And when a/the client requests something the server is quick to respond.
670  int slept;
671  int sleepincus = 1000; //1ms maximum to sleep in one shot - small enough server remains responsive to client
672  int maxsleepus = 1000000; //1s - max to sleep when no SSRClient requets, between fwl_draw()s
673  slept = 0;
674  while(!item && slept < maxsleepus) {
675  item = threadsafe_dequeue_item(&ssr_queue, &ssr_queue_mutex );
676  if(!item){
677  usleep(sleepincus);
678  slept += sleepincus;
679  }
680  }
681  }else if(ssr_queue){
682  item = threadsafe_dequeue_item(&ssr_queue, &ssr_queue_mutex );
683  }
684  if(item){
685  request = item->elem;
686  //free(item);
687  switch(request->type)
688  {
689  case SSR_INITPOSE:
690  break;
691  case SSR_POSEPOSE:
692  case SSR_POSESNAPSHOT:
693  SSR_set_pose(request);
694  break;
695  default:
696  break;
697  }
698  }
699  ssr_current_request = request;
700 
701 }
702 
703 
704 #ifdef _MSC_VER
705 static char *snapshot_filename = "snapshot.bmp"; //option: get this from the snapshot.c module after saving
706 #else
707 static char *snapshot_filename = "snapshot.png";
708 #endif
709 void Snapshot1(char *fname);
710 void SSR_reply_snapshot(SSR_request *request)
711 {
712  int iret;
713  Snapshot1(snapshot_filename); //win32 has Snapshot1 with filename, need to add to linux
714  iret = load_file_blob(snapshot_filename,&request->blob,&request->len);
715  if(!iret)
716  printf("snapshot file not found %s\n",snapshot_filename);
717  else
718  unlink(snapshot_filename);
719 
720 }
721 void SSR_reply(){
722  //called by D: _DisplayThread at end of draw loop (or just before SSR_dequeue_request at start of next loop)
723  //only one ssr_current_request is processed per frame/draw loop
724  if(ssr_current_request){
725  SSR_request *request = ssr_current_request;
726  switch(request->type){
727  case SSR_INITPOSE:
728  SSR_reply_pose(request,TRUE); break;
729  case SSR_POSEPOSE:
730  SSR_reply_pose(request,FALSE); break;
731  case SSR_POSESNAPSHOT:
732  SSR_reply_snapshot(request); break;
733  default:
734  break;
735  }
736  //tell waiting B server thread to wake up and reply to A html client
737  request->answered = 1;
738  pthread_cond_signal(&request->requester_condition);
739  ssr_current_request = NULL;
740  }
741 }
742 #endif //SSR_SERVER
SSR_request
Definition: SSRhelper.h:7
_s_list_t
Definition: list.h:37
keyval
Definition: common.c:52
key
Definition: Viewer.h:174