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