FreeWRL / FreeX3D 4.3.0
Component_Interpolation.c
1
2/****************************************************************************
3 This file is part of the FreeWRL/FreeX3D Distribution.
4
5 Copyright 2009 CRC Canada. (http://www.crc.gc.ca)
6
7 FreeWRL/FreeX3D is free software: you can redistribute it and/or modify
8 it under the terms of the GNU Lesser Public License as published by
9 the Free Software Foundation, either version 3 of the License, or
10 (at your option) any later version.
11
12 FreeWRL/FreeX3D is distributed in the hope that it will be useful,
13 but WITHOUT ANY WARRANTY; without even the implied warranty of
14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 GNU General Public License for more details.
16
17 You should have received a copy of the GNU General Public License
18 along with FreeWRL/FreeX3D. If not, see <http://www.gnu.org/licenses/>.
19****************************************************************************/
20
21
22/*******************************************************************
23
24 X3D Interpolation Component
25
26*********************************************************************/
27
28
29#include <config.h>
30#include <system.h>
31#include <display.h>
32#include <internal.h>
33
34#include <libFreeWRL.h>
35#include <list.h>
36
37#include "../vrml_parser/Structs.h"
38#include "../input/InputFunctions.h"
39#include "../opengl/LoadTextures.h" /* for finding a texture url in a multi url */
40
41
42#include "../main/headers.h"
43#include "../opengl/OpenGL_Utils.h"
44#include "../scenegraph/RenderFuncs.h"
45
46#include "../x3d_parser/Bindable.h"
47#include "../scenegraph/LinearAlgebra.h"
48#include "../scenegraph/Collision.h"
49#include "../scenegraph/quaternion.h"
50#include "../vrml_parser/CRoutes.h"
51#include "../opengl/OpenGL_Utils.h"
52#include "../opengl/Textures.h" /* for finding a texture url in a multi url */
53
54#include "../input/SensInterps.h"
55
56//defined in Component_RigidBodyPhysics
57int NNC0(struct X3D_Node* node);
58void MNC0(struct X3D_Node* node);
59void MNX0(struct X3D_Node* node);
60#define NNC(A) NNC0(X3D_NODE(A)) //node needs compiling
61#define MNC(A) MNC0(X3D_NODE(A)) //mark node compiled
62// #define MNX(A) MNX0(X3D_NODE(A)) //mark node changed
63// #define PPX(A) getTypeNode(X3D_NODE(A)) //possible proto expansion
64
65// http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/interp.html
66// see also SenseInterps.c for other interpolator componenent implementations
67
68void do_EaseInEaseOut(void *node){
69 // http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/interp.html#EaseInEaseOut
70
71 /* ScalarInterpolator - store final value in px->value_changed */
72 struct X3D_EaseInEaseOut *px;
73 int kin, ispan; //kvin,
74 //float *kVs;
75 //int counter;
76 float fraction, u, eout, ein, S;
77
78
79 if (!node) return;
80 px = (struct X3D_EaseInEaseOut *) node;
81 kin = px->key.n;
82
83 MARK_EVENT (node, offsetof (struct X3D_EaseInEaseOut, modifiedFraction_changed));
84
85 fraction = min(px->set_fraction,px->key.p[kin-1]);
86 fraction = max(px->set_fraction,px->key.p[0]);
87 /* have to go through and find the key before */
88 ispan =find_key(kin,fraction,px->key.p);
89 u = (fraction - px->key.p[ispan-1]) / (px->key.p[ispan] - px->key.p[ispan-1]);
90 eout = px->easeInEaseOut.p[ispan].c[1]; //y
91 ein = px->easeInEaseOut.p[ispan+1].c[0]; //x
92 S = eout + ein;
93
94 if(S < 0.0f){
95 px->modifiedFraction_changed = u;
96 }else{
97 float t;
98 if(S > 1.0f){
99 ein /= S;
100 eout /= S;
101 }
102 t = 1.0f / (2.0f - eout - ein);
103 if(u < eout){
104 px->modifiedFraction_changed = (t/eout) * u*u;
105 }else if(u < 1.0f - ein){
106 px->modifiedFraction_changed = (t*(2*u - eout));
107 }else{
108 px->modifiedFraction_changed = 1.0f - (t*(1.0f - u)*(1.0f - u))/ein;
109 }
110 }
111
112
113}
114
115/*
116 Splines via Hermite
117 http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/interp.html#HermiteSplineInterpolation
118 https://en.wikipedia.org/wiki/Cubic_Hermite_spline
119 - you give it a slope and position at each end, and a 0-1 t
120 - you can compute for xyz by doing 3 separate scalar interpolations one for each coordinate
121 - or to save FLOPS (floating point ops) you can do a template algo like we did for Followers
122
123 if linear interp looks like this:
124 fraction s = fraction_interp(t,t0,t1);
125 answer = linear_interp(s,key0,val0,key1,val1)
126
127 Then spline interp would look like this:
128 fraction s = fraction_interp(t,t0,t1);
129 answer = spline_interp(s,key0,val0,vel0,key1,val1,vel1)
130 - where vel is velocity or more generally slope/first-derivitive of value at key.
131 -- and first derivitive would be with respect to key ie d(Value)/d(key) evaluated at key
132 -- and according to specs you calculate defaults if user doesn't supply
133 - and in general {answer, val*, vel*} are vectors or types ie SFVec3f, SFRotation, SFfloat
134
135*/
136static void spline_interp(int dim, float *result, float s, float *val0, float *val1, float *T00, float *T11){
137 // dim - dimension of val and vel ie 3 for xyz
138 // s - span fraction 0-1
139 // interpolates one value, given the span values
140 float S[4], *C[4], SH[4];
141 int i,j;
142 static float H[16] = {
143 2.0f, -3.0f, 0.0f, 1.0f,
144 -2.0f, 3.0f, 0.0f, 0.0f,
145 1.0f, -2.0f, 1.0f, 0.0f,
146 1.0f, -1.0f, 0.0f, 0.0f};
147
148
149 S[0] = s*s*s;
150 S[1] = s*s;
151 S[2] = s;
152 S[3] = 1.0f;
153 vecmultmat4f(SH,S,H);
154
155 C[0] = val0; //vi
156 C[1] = val1; //vi+1
157 C[2] = T00; //T0i
158 C[3] = T11; //T1i+1
159
160 for(i=0;i<dim;i++){
161 result[i] = 0.0f;
162 for(j=0;j<4;j++){
163 result[i] += SH[j]*C[j][i];
164 }
165 }
166}
167
168//for xyz dim =3, for xy dim=2 for scalar dim=1
169static void compute_spline_velocity_Ti(int dim, int normalize, int nval, float *val, int nvel, float* vel, float *Ti ){
170 //please malloc Ti = malloc(nval*dim*sizeof(float)) before calling
171 if(nvel && vel && nvel == nval){
172 if(!normalize){
173 //If the velocity vector is specified, and the normalizeVelocity flag has value FALSE,
174 //the velocity at the key is set to the corresponding value of the keyVelocity field:
175 //Ti = keyVelocity[ i ]
176 memcpy(Ti,vel,dim*nval*sizeof(float));
177 }else{
178 //If the velocity vector is specified, and the normalizeVelocity flag is TRUE,
179 // the velocity at the key is set using the corresponding value of the keyVelocity field:
180 //Ti = keyVelocity[i] × ( Dtot / |keyVelocity[i]| )
181 int i;
182 //Dtot = SUM{i=0, i < n-1}(|vi - vi+1|)
183 float Dtot = 0.0f;
184 for(i=0;i<nval-1;i++){
185 float Di = 0.0f;
186 int j;
187 for(j=0;j<dim;j++){
188 Di += (val[i+1] - val[i])*(val[i+1] - val[i]); //euclidean distance d= sqrt(x**2 + y**2)
189 }
190 Dtot += (float)sqrt(Di);
191 }
192 for(i=0;i<nval;i++){
193 int j;
194 float veli = 0.0f;
195 for(j=0;j<dim;j++)
196 veli = veli + (vel[i*dim + j]*vel[i*dim + j]); //euclidean distance d= sqrt(x**2 + y**2)
197 veli = (float)sqrt(veli);
198 for(j=0;j<dim;j++){
199 if(veli != 0.0f)
200 Ti[i*dim + j] = Dtot * vel[i*dim + j] / veli;
201 else
202 Ti[i*dim + j] = 0.0f;
203 }
204 }
205 }
206 }else{
207 //If the velocity vector is not specified, (or just start & end,) it is calculated as follows:
208 //Ti = (vi+1 - vi-1) / 2
209 int i;
210 if(nvel == 2){
211 int j;
212 for(j=0;j<dim;j++){
213 Ti[ 0*dim + j] = vel[0*dim + j];
214 Ti[(nval-1)*dim + j] = vel[1*dim + j];
215 }
216
217 }else{
218 int j;
219 for(j=0;j<dim;j++){
220 Ti[ 0*dim + j] = 0.0f;
221 Ti[(nval-1)*dim + j] = 0.0f;
222 }
223 }
224 //skip start and end vels here
225 for(i=1;i<nval-1;i++){
226 int j;
227 for(j=0;j<dim;j++)
228 Ti[i*dim + j] = (val[(i+1)*dim +j] - val[(i-1)*dim +j]) * .5f;
229 }
230 }
231}
232
233int iwrap(int i, int istart, int iend){
234 //if they don't duplicate - the last point != first point - then iend = n
235 //if they duplicate last point == first point, then iend = n-1
236 // normally istart = 0, iend = n
237 // 6 = iwrap(-1,0,7)
238 // 0 = iwrap(7,0,7)
239 int iret = i;
240 if(iret < istart) iret = iend - (istart - iret);
241 if(iret >= iend ) iret = istart + (iret - iend);
242 return iret;
243}
244static void spline_velocity_adjust_for_keyspan(int dim, int closed, int nval, float *key, float *Ti, float* T0, float *T1){
245 //before calling please malloc T0 and T1 = malloc(nval * dim * sizeof(float))
246 float Fp, Fm;
247 int istart, iend, jend,i,j;
248 istart = 1;
249 iend = nval-1;
250 jend = nval;
251 if(closed){
252 istart = 0;
253 iend = nval;
254 jend = nval-1; //the first and last point are duplicates, so when wrapping, skip the last
255 }else{
256 //take first and last values from Ti which were either 0 or start/end
257 int l = nval-1;
258 for(j=0;j<dim;j++){
259 T1[0*dim +j] = T0[0*dim +j] = Ti[0*dim +j];
260 T1[l*dim +j] = T0[l*dim +j] = Ti[l*dim +j];
261 }
262 }
263 for(i=istart;i<iend;i++){
264 int ip, im;
265 ip = iwrap(i+1,0,jend);
266 im = iwrap(i-1,0,jend);
267 Fm = 2.0f*(key[ip] - key[i])/(key[ip]-key[im]);
268 Fp = 2.0f*(key[i] - key[im])/(key[ip]-key[im]);
269 for(j=0;j<dim;j++){
270 T0[i*dim +j] = Fp*Ti[i*dim +j];
271 T1[i*dim +j] = Fm*Ti[i*dim +j];
272 }
273 }
274}
275
276static float span_fraction(float t, float t0, float t1){
277 float ret;
278 ret = (t - t0) /(t1 - t0);
279 return ret;
280}
281
282void do_SplinePositionInterpolator(void *node){
283 // http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/interp.html#SplinePositionInterpolator
284 int dim;
285 int kin, kvin;
286 int ispan; //counter,
287 float fraction, sfraction;
288
290 dim = 3;
291 if(NNC(px)){
292
293 //void compute_spline_velocity_Ti(int dim, int normalize, int nval, float *val, int nvel, float* vel, float *Ti )
294 int isclosed,n;
295 float *Ti, *T0, *T1;
296 n = px->key.n;
297 Ti = MALLOC(float*,n*dim*sizeof(float));
298 compute_spline_velocity_Ti(dim,px->normalizeVelocity,n,(float*)px->keyValue.p,
299 px->keyVelocity.n,(float*)px->keyVelocity.p,Ti);
300 T0 = MALLOC(float*,n*dim*sizeof(float));
301 T1 = MALLOC(float*,n*dim*sizeof(float));
302 //spline_velocity_adjust_for_keyspan(int dim, int closed, int nval, float *key, float *Ti, float* T0, float *T1)
303 isclosed = px->closed && vecsame3f(px->keyValue.p[0].c,px->keyValue.p[n-1].c);
304 spline_velocity_adjust_for_keyspan(dim,isclosed,n,px->key.p,Ti,T0,T1);
305 FREE_IF_NZ(px->_T0.p);
306 FREE_IF_NZ(px->_T1.p);
307 px->_T0.p = (struct SFVec3f*)T0;
308 px->_T0.n = n;
309 px->_T1.p = (struct SFVec3f*)T1;
310 px->_T1.n = n;
311 FREE_IF_NZ(Ti);
312
313 MNC(px);
314 }
315 //void spline_interp(int dim, float *result, float s, float *val0, float *val1, float *T00, float *T11)
316
317 if (!node) return;
318 kin = px->key.n;
319 kvin = px->keyValue.n;
320
321 MARK_EVENT (node, offsetof (struct X3D_SplinePositionInterpolator, value_changed));
322
323 /* make sure we have the keys and keyValues */
324 if ((kvin == 0) || (kin == 0)) {
325 vecset3f(px->value_changed.c,0.0f,0.0f,0.0f);
326 return;
327 }
328 if (kin>kvin) kin=kvin; /* means we don't use whole of keyValue, but... */
329
330 #ifdef SEVERBOSE
331 printf ("ScalarInterpolator, kin %d kvin %d, vc %f\n",kin,kvin,px->value_changed);
332 #endif
333
334 /* set_fraction less than or greater than keys */
335 fraction = min(px->set_fraction,px->key.p[kin-1]);
336 fraction = max(px->set_fraction,px->key.p[0]);
337 /* have to go through and find the key before */
338 ispan =find_key(kin,fraction,px->key.p) -1;
339
340 // INTERPOLATION FUNCTION - change this from linear to spline
341 // fraction s = fraction_interp(t,t0,t1);
342 // answer = linear_interp(s,key0,val0,key1,val1)
343 // answer = spline_interp(s,key0,val0,vel0,key1,val1,vel1)
344 // where vel is velocity or more generally slope/first-derivitive of value at key.
345 // First derivitive would be with respect to key ie d(Value)/d(key) evaluated at key
346 // in general answer, val*, vel* are vectors or types.
347 sfraction = span_fraction(fraction,px->key.p[ispan],px->key.p[ispan+1]);
348 spline_interp(dim, px->value_changed.c, sfraction,
349 px->keyValue.p[ispan].c, px->keyValue.p[ispan+1].c,
350 px->_T0.p[ispan].c, px->_T1.p[ispan+1].c);
351}
352
353void do_SplinePositionInterpolator2D(void *node){
354 int dim;
355 int kin, kvin;
356 int ispan; //counter,
357 float fraction,sfraction;
358
360 dim = 2;
361 if(NNC(px)){
362
363 //void compute_spline_velocity_Ti(int dim, int normalize, int nval, float *val, int nvel, float* vel, float *Ti )
364 int isclosed,n;
365 float *Ti,*T0,*T1;
366 n = px->key.n;
367 Ti = MALLOC(float*,n*dim*sizeof(float));
368 compute_spline_velocity_Ti(dim,px->normalizeVelocity,n,(float*)px->keyValue.p,
369 px->keyVelocity.n,(float*)px->keyVelocity.p,Ti);
370 T0 = MALLOC(float*,n*dim*sizeof(float));
371 T1 = MALLOC(float*,n*dim*sizeof(float));
372 //spline_velocity_adjust_for_keyspan(int dim, int closed, int nval, float *key, float *Ti, float* T0, float *T1)
373 isclosed = px->closed && vecsame2f(px->keyValue.p[0].c,px->keyValue.p[n-1].c);
374 spline_velocity_adjust_for_keyspan(dim,isclosed,n,px->key.p,Ti,T0,T1);
375 FREE_IF_NZ(px->_T0.p);
376 FREE_IF_NZ(px->_T1.p);
377 px->_T0.p = (struct SFVec2f*)T0;
378 px->_T0.n = n;
379 px->_T1.p = (struct SFVec2f*)T1;
380 px->_T1.n = n;
381 FREE_IF_NZ(Ti);
382
383 MNC(px);
384 }
385 //void spline_interp(int dim, float *result, float s, float *val0, float *val1, float *T00, float *T11)
386
387 if (!node) return;
388 kin = px->key.n;
389 kvin = px->keyValue.n;
390
391 MARK_EVENT (node, offsetof (struct X3D_SplinePositionInterpolator2D, value_changed));
392
393 /* make sure we have the keys and keyValues */
394 if ((kvin == 0) || (kin == 0)) {
395 vecset2f(px->value_changed.c,0.0f,0.0f);
396 return;
397 }
398 if (kin>kvin) kin=kvin; /* means we don't use whole of keyValue, but... */
399
400 #ifdef SEVERBOSE
401 printf ("ScalarInterpolator, kin %d kvin %d, vc %f\n",kin,kvin,px->value_changed);
402 #endif
403
404 /* set_fraction less than or greater than keys */
405 fraction = min(px->set_fraction,px->key.p[kin-1]);
406 fraction = max(px->set_fraction,px->key.p[0]);
407 /* have to go through and find the key before */
408 ispan =find_key(kin,fraction,px->key.p) -1;
409
410 // INTERPOLATION FUNCTION - change this from linear to spline
411 // fraction s = fraction_interp(t,t0,t1);
412 // answer = linear_interp(s,key0,val0,key1,val1)
413 // answer = spline_interp(s,key0,val0,vel0,key1,val1,vel1)
414 // where vel is velocity or more generally slope/first-derivitive of value at key.
415 // First derivitive would be with respect to key ie d(Value)/d(key) evaluated at key
416 // in general answer, val*, vel* are vectors or types.
417 sfraction = span_fraction(fraction,px->key.p[ispan],px->key.p[ispan+1]);
418 spline_interp(dim, px->value_changed.c, sfraction,
419 px->keyValue.p[ispan].c, px->keyValue.p[ispan+1].c,
420 px->_T0.p[ispan].c, px->_T1.p[ispan+1].c);
421
422}
423
424void do_SplineScalarInterpolator(void *node){
425 // SplineScalarInterpolator - store final value in px->value_changed
426 // - body of function copied from ScalarInterpolator and node cast to SplineScalarInterpolator
427 int dim;
428 int kin, kvin;
429 int ispan; //counter,
430 float fraction, sfraction;
431
433 dim = 1;
434 if(NNC(px)){
435
436 //void compute_spline_velocity_Ti(int dim, int normalize, int nval, float *val, int nvel, float* vel, float *Ti )
437 int isclosed, i, n;
438 float *Ti,*T0,*T1;
439
440 n = px->key.n;
441 Ti = MALLOC(float*,n*dim*sizeof(float));
442 compute_spline_velocity_Ti(dim,px->normalizeVelocity,n,(float*)px->keyValue.p,
443 px->keyVelocity.n,(float*)px->keyVelocity.p,Ti);
444 printf("\nvelocities\n");
445 for(i=0;i<n;i++)
446 printf("%f ",Ti[i]);
447 printf("\n");
448 T0 = MALLOC(float*,n*dim*sizeof(float));
449 T1 = MALLOC(float*,n*dim*sizeof(float));
450 //spline_velocity_adjust_for_keyspan(int dim, int closed, int nval, float *key, float *Ti, float* T0, float *T1)
451 isclosed = px->closed && px->keyValue.p[0] == px->keyValue.p[n-1];
452 spline_velocity_adjust_for_keyspan(1,isclosed,n,px->key.p,Ti,T0,T1);
453 for(i=0;i<n;i++)
454 printf("%f ",Ti[i]);
455 printf("\n");
456 FREE_IF_NZ(px->_T0.p);
457 FREE_IF_NZ(px->_T1.p);
458 px->_T0.p = T0;
459 px->_T0.n = n;
460 px->_T1.p = T1;
461 px->_T1.n = n;
462 FREE_IF_NZ(Ti);
463
464 MNC(px);
465 }
466 //void spline_interp(int dim, float *result, float s, float *val0, float *val1, float *T00, float *T11)
467
468 if (!node) return;
469 kin = px->key.n;
470 kvin = px->keyValue.n;
471
472 MARK_EVENT (node, offsetof (struct X3D_SplineScalarInterpolator, value_changed));
473
474 /* make sure we have the keys and keyValues */
475 if ((kvin == 0) || (kin == 0)) {
476 px->value_changed = 0.0;
477 return;
478 }
479 if (kin>kvin) kin=kvin; /* means we don't use whole of keyValue, but... */
480
481 #ifdef SEVERBOSE
482 printf ("ScalarInterpolator, kin %d kvin %d, vc %f\n",kin,kvin,px->value_changed);
483 #endif
484
485 /* set_fraction less than or greater than keys */
486 fraction = min(px->set_fraction,px->key.p[kin-1]);
487 fraction = max(px->set_fraction,px->key.p[0]);
488 /* have to go through and find the key before */
489 ispan =find_key(kin,fraction,px->key.p) -1;
490
491 // INTERPOLATION FUNCTION - change this from linear to spline
492 // fraction s = fraction_interp(t,t0,t1);
493 // answer = linear_interp(s,key0,val0,key1,val1)
494 // answer = spline_interp(s,key0,val0,vel0,key1,val1,vel1)
495 // where vel is velocity or more generally slope/first-derivitive of value at key.
496 // First derivitive would be with respect to key ie d(Value)/d(key) evaluated at key
497 // in general answer, val*, vel* are vectors or types.
498 sfraction = span_fraction(fraction,px->key.p[ispan],px->key.p[ispan+1]);
499 spline_interp(dim, (float*)&px->value_changed, sfraction,
500 &px->keyValue.p[ispan], &px->keyValue.p[ispan+1],
501 &px->_T0.p[ispan], &px->_T1.p[ispan+1]);
502
503}
504
505
506// START MIT LIC >>>>
507
508static double *quaternion2double(double *xyzw, const Quaternion *q){
509 xyzw[0] = q->x;
510 xyzw[1] = q->y;
511 xyzw[2] = q->z;
512 xyzw[3] = q->w;
513 return xyzw;
514}
515static Quaternion *double2quaternion(Quaternion *q, const double* xyzw){
516 q->x = xyzw[0];
517 q->y = xyzw[1];
518 q->z = xyzw[2];
519 q->w = xyzw[3];
520 return q;
521}
522static void quaternion_addition(Quaternion *ret, const Quaternion *q1, const Quaternion *q2){
523 //the quaternion_add in Quanternions.c seems too complicated.
524 // supposed to be q+q' = [s+s',v+v']
525 double xyzw1[4],xyzw2[4],xyzw[4];
526 quaternion2double(xyzw1,q1);
527 quaternion2double(xyzw2,q2);
528 vecaddd(xyzw,xyzw1,xyzw2);
529 xyzw[3] = xyzw1[3] + xyzw2[3];
530 double2quaternion(ret,xyzw);
531}
532static void quaternion_subtraction(Quaternion *ret, const Quaternion *q1, const Quaternion *q2){
533 // supposed to be q-q' = [s-s',v-v']
534 double xyzw1[4],xyzw2[4],xyzw[4];
535 quaternion2double(xyzw1,q1);
536 quaternion2double(xyzw2,q2);
537 vecdifd(xyzw,xyzw1,xyzw2);
538 xyzw[3] = xyzw1[3] - xyzw2[3];
539 double2quaternion(ret,xyzw);
540}
541static double fwclampd(const double val, const double low, const double hi){
542 double ret = val;
543 ret = min(ret,hi);
544 ret = max(ret,low);
545 return ret;
546}
547static double quaternion_dot(const Quaternion *q1, const Quaternion *q2){
548 double xyzw1[4], xyzw2[4], dot;
549 int i;
550 quaternion2double(xyzw1,q1);
551 quaternion2double(xyzw2,q1);
552 dot = 0.0;
553 for(i=0;i<4;i++)
554 dot += xyzw1[i]*xyzw2[i];
555 return dot;
556}
557static void quaternion_negate(Quaternion *q){
558 int i;
559 double xyzw[4];
560 quaternion2double(xyzw,q);
561 for(i=0;i<4;i++)
562 xyzw[i] = -xyzw[i];
563 double2quaternion(q,xyzw);
564}
565static void quaternion_log(Quaternion *ret, const Quaternion *q){
566 double angle, angle_over_sine, xyzw[4];
567
568 quaternion2double(xyzw,q);
569 angle = acos( fwclampd(xyzw[3],-1.0,1.0));
570 angle_over_sine = 0.0;
571 if(angle != 0.0) angle_over_sine = angle/sin(angle);
572 vecscaled(xyzw,xyzw,angle_over_sine);
573 xyzw[3]=0.0;
574 double2quaternion(ret,xyzw);
575}
576static void quaternion_exp(Quaternion *ret, const Quaternion *q){
577 double angle, xyzw[4], sine_over_angle;
578
579 quaternion2double(xyzw,q);
580 angle = veclengthd(xyzw);
581 sine_over_angle = 0.0;
582 if(angle != 0.0) sine_over_angle = sin(angle) / angle;
583 vecscaled(xyzw,xyzw,sine_over_angle);
584 xyzw[3] = cos(angle);
585 double2quaternion(ret,xyzw);
586}
587static void compute_si(Quaternion *si,Quaternion *qi, const Quaternion *qip1, const Quaternion *qim1){
588 //si is like the (quaternion-space) slope at point qi, or like a bezier tangent control point
589 //and is supposed to be the average from either side of (quaternion-space) point qi
590 //or more precisely, the 2 slopes / 1st derivitives are set equal
591 // http://web.mit.edu/2.998/www/QuaternionReport1.pdf
592 // si = qi * exp( - (log(inv(qi)*qi+1) + log(inv(qi)*qi-1)) / 4 )
593 // and qi+1 means q[i+1] and qi-1 means q[i-1] ie that's an indexer, not a quat + scalar
594 // p. 15 shows log(q) if q=[cost,sint*v] then
595 // log(q) = [0, t*v] (non-unit-sphere in quaternion space, not in H1)
596 // exp(q) = [cost, sint*v] (puts cos(t) back in scalar part, inverting log)
597 // http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
598 // p.10 "the logarithm of a unit quaternion q = [vˆ sin O, cos O] is just vˆO, a pure vector of length O."
599
600 Quaternion qiinv, qiinv_qip1, qiinv_qim1,qadded,qexp;
601 double xyzw[4];
602
603 quaternion_inverse(&qiinv,qi);
604 quaternion_multiply(&qiinv_qip1,&qiinv,qip1);
605
606 quaternion_log(&qiinv_qip1,&qiinv_qip1);
607 quaternion_multiply(&qiinv_qim1,&qiinv,qim1);
608 quaternion_log(&qiinv_qim1,&qiinv_qim1);
609
610 quaternion_addition(&qadded,&qiinv_qip1,&qiinv_qim1); //don't normalize - its still a log, which aren't H1
611 quaternion2double(xyzw,&qadded);
612 vecscaled(xyzw,xyzw,-.25); // -ve and /4
613 xyzw[3] *= -.25; //I think this will still be 0 after adding 2 logs
614
615 //compute exp
616 double2quaternion(&qexp,xyzw);
617 quaternion_exp(&qexp,&qexp);
618
619 //quaternion_normalize(&qexp); //can normalize now, back to H1
620 quaternion_multiply(si,qi,&qexp);
621
622}
623
624static void debug_SquadOrientationInterpolator(struct X3D_SquadOrientationInterpolator *px){
625 //just a mess of crap, plus: normalization of axisangle axis
626 // qinterp = Squad(qi, qi+1,si,si+1,h) = Slerp( Slerp(qi,qi+1,h), Slerp(si,si+1,h), 2h(1-h))
627 //in theory, the vrmlrot_to_quaternion and compute_si can be done in a compile_squadorientationinterpolator step
628 //and the si and quats for each keyvalue cached
629 Quaternion qi,qip1,qip2,qim1,si,sip1;
630 int ip1, ip2, im1, kin, iend,i;
631 struct SFRotation *kVs = px->keyValue.p;
632 static int once_debug = 0;
633 if(once_debug != 0) return;
634 once_debug = 1;
635 kin = px->key.n;
636
637 //for wrap-around if closed, adjust the wrap around end value based on
638 //whether the scene author duplicated the first keyvalue in the last.
639 iend = kin;
640 if(vecsame4f(px->keyValue.p[0].c,px->keyValue.p[kin-1].c)) iend = kin -1;
641
642 //there are 1 fewer spans than key/values
643 //we'll iterate over key/values here
644 printf("Si:\n");
645 for(i=0;i<kin;i++){
646 float axislength;
647 if(1) printf("%d ri [%f %f %f, %f]\n",i,kVs[i].c[0], kVs[i].c[1], kVs[i].c[2], kVs[i].c[3]);
648 axislength = veclength3f(kVs[i].c);
649 if(axislength != 0.0f)
650 vecscale3f(kVs[i].c,kVs[i].c,1.0f/axislength);
651 if(1) printf("%d ri [%f %f %f, %f]\n",i,kVs[i].c[0], kVs[i].c[1], kVs[i].c[2], kVs[i].c[3]);
652
653 vrmlrot_to_quaternion (&qi, kVs[i].c[0], kVs[i].c[1], kVs[i].c[2], kVs[i].c[3]);
654 ip1 = i+1;
655 ip1 = px->closed ? iwrap(ip1,0,iend) : min(max(ip1,0),kin-2);
656 vrmlrot_to_quaternion (&qip1, kVs[ip1].c[0],kVs[ip1].c[1], kVs[ip1].c[2], kVs[ip1].c[3]);
657 if(0){
658 ip2 =i+2;
659 ip2 = px->closed ? iwrap(ip2,0,iend) : min(max(ip2,0),kin-1);
660 vrmlrot_to_quaternion (&qip2, kVs[ip2].c[0],kVs[ip2].c[1], kVs[ip2].c[2], kVs[ip2].c[3]);
661 }
662 im1 = i-1;
663 im1 = px->closed ? iwrap(im1,0,iend) : min(max(im1,0),kin-3);
664 vrmlrot_to_quaternion (&qim1, kVs[im1].c[0],kVs[im1].c[1], kVs[im1].c[2], kVs[im1].c[3]);
665 //si
666 compute_si(&si,&qi, &qip1, &qim1);
667 if(0){
668 compute_si(&sip1,&qip1,&qip2,&qi);
669 printf("%d qi [%lf, %lf %lf %lf]\n",i,qi.w,qi.x,qi.y,qi.z);
670 printf("%d qi+1 [%lf, %lf %lf %lf]\n",i,qip1.w,qip1.x,qip1.y,qip1.z);
671 printf("%d si [%lf, %lf %lf %lf]\n",i,si.w,si.x,si.y,si.z);
672 printf("%d si+1 [%lf, %lf %lf %lf]\n",i,sip1.w,sip1.x,sip1.y,sip1.z);
673 }else{
674 double xyza[4];
675 quaternion_to_vrmlrot(&si,&xyza[0],&xyza[1],&xyza[2],&xyza[3] );
676 //printf("%lf %lf %lf %lf, \n",xyza[0],xyza[1],xyza[2],xyza[3]);
677 }
678
679
680 }
681 printf("\n");
682}
683
684static void quaternion_lerp(Quaternion *ret, const Quaternion *q1, const Quaternion *q2, const double t){
685 Quaternion t1, t2;
686 t1 = *q1;
687 t2 = *q2;
688 quaternion_scalar_multiply(&t2,t);
689 quaternion_scalar_multiply(&t1,1.0 -t);
690 quaternion_addition(ret,&t1,&t2);
691}
692static void quaternion_slerp2(Quaternion *ret, const Quaternion *q1, const Quaternion *q2, const double t){
693 double dot, dn1,dn2;
694 Quaternion r;
695
696 dn1 = quaternion_norm(q1);
697 dn2 = quaternion_norm(q2);
698 dot = quaternion_dot(q1,q2);
699 if(dn1 != 0.0 && dn2 != 0.0) dot = dot /(dn1*dn2); //no improvement to round-the-world or kinks
700 r = *q2;
701 if(dot < 0.0){
702 dot = -dot;
703 quaternion_negate(&r);
704 }
705 if(1.0 - dot < .001){
706 quaternion_lerp(ret,q1,&r,t);
707 }else{
708 Quaternion t1, t2;
709 double angle = acos(dot);
710 t1 = *q1;
711 t2 = r;
712 quaternion_scalar_multiply(&t1,sin((1.0-t)*angle));
713 quaternion_scalar_multiply(&t2,sin(t*angle));
714 quaternion_addition(ret,&t1,&t2);
715 // if(ret->w < 0.0) quaternion_negate(ret); //CQRlib Hlerp quirk - no improvement to round-the-world or kinks
716 quaternion_scalar_multiply(ret,1.0/sin(angle));
717 }
718
719}
720
721static void quaternion_squad_prepare(Quaternion *qim1,Quaternion *qi,Quaternion *qip1,Quaternion *qip2,
722 Quaternion *s1,Quaternion *s2,Quaternion *qc){
723 Quaternion qp,qm, q0,q1,q2,q3;
724 q0 = *qim1;
725 q1 = *qi;
726 q2 = *qip1;
727 q3 = *qip2;
728
729 //microsoft directx squad does something like this before computing si,
730 //to avoid round-the-world problems
731 //q0 = |q0 + q1| < |q0 - q1| ? -q0 : q0
732 //q2 = |q1 + q2| < |q1 - q2| ? -q2 : q2
733 //q3 = |q2 + q3| < |q2 - q3| ? -q3 : q3
734 quaternion_addition(&qp,&q0,&q1);
735 quaternion_subtraction(&qm,&q0,&q1);
736 if( quaternion_norm(&qp) < quaternion_norm(&qm) ) quaternion_negate(&q0);
737 quaternion_addition(&qp,&q1,&q2);
738 quaternion_subtraction(&qm,&q1,&q2);
739 if( quaternion_norm(&qp) < quaternion_norm(&qm) ) quaternion_negate(&q2);
740 quaternion_addition(&qp,&q2,&q3);
741 quaternion_subtraction(&qm,&q2,&q3);
742 if( quaternion_norm(&qp) < quaternion_norm(&qm) ) quaternion_negate(&q3);
743
744 compute_si(s1,&q1,&q2,&q0);
745 compute_si(s2,&q2,&q3,&q1);
746 *qc = q2; //qip1 could have changed sign, use the sign-changed version in squad slerping
747
748}
749static void quaternion_squad(Quaternion *final,Quaternion *q1,Quaternion *q2, Quaternion *s1,Quaternion *s2,double t){
750 Quaternion qs, ss;
751 quaternion_slerp2(&qs,q1,q2,t); //qip1 replaceed with possibly negated qc, helps test D
752 quaternion_slerp2(&ss,s1,s2,t);
753 quaternion_slerp2(final, &qs, &ss, 2.0*t*(1.0 -t));
754 quaternion_normalize(final);
755}
756static void quaternion_diff(Quaternion *qdiff, Quaternion *q2, Quaternion *q1){
757 // diff * q1 = q2 ---> diff = q2 * inverse(q1)
758 //where: inverse(q1) = conjugate(q1) / abs(q1)}
759 Quaternion q1inv;
760 quaternion_inverse(&q1inv,q1);
761 quaternion_multiply(qdiff,q2,&q1inv);
762 if(qdiff->w < 0.0){
763 quaternion_negate(&q1inv);
764 quaternion_multiply(qdiff,q2,&q1inv);
765 }
766
767}
768static void squad_compute_velocity_normalized_key_keyvalue(int closed,
769 int n, float *keys, float *values,
770 int *nnorm, float **normkeys, float **normvals)
771{
772 //velocity is in radians per fraction
773 //the idea is to adjust the keys, or the values, so that all spans have the same velocity
774 //methods:
775 //1. equal keys method: decide on keys per radian, and malloc round_up(total radians) * keys per radian
776 // then find the value that goes with each key
777 // a) closest from precalculated fine mini-spans (and take the key that goes with it)
778 // b) iterate with guess, compute, closer guess, compute until cumulative angle matches cumulative fraction
779 //2. adjusted keys method:
780 // adjust current keys so current spans have the same velocity
781 // problem: discontiniutiy in velocity / abrupt change of velocity at key/value points
782 //problem with both 1 and 2:
783 // the angular velocity includes yaw, pitch and roll. If you are wanting
785 //algorithm: (dug9, made up)
786 //1. iterate over all spans, 10 or 1000 fractions per span, summing up the angular distance for each span
787 //2. compute how much each span gets fraction-wise
788 //3. malloc new keys and values
789 //4. compute new keys and or values
790 Quaternion qlast, *mvals;
791 double totalangle, angle, velocity;
792 int i,j,k, iend, nspan;
793 float *mkeys,*cumangle, *spanangle, *nkey, *nvals, cumkey;
794 struct SFRotation *kVs;
795
796 mkeys = MALLOC(float*,n*100*sizeof(float));
797 mvals = MALLOC(Quaternion*,n*100*sizeof(Quaternion));
798 cumangle = MALLOC(float*,n*100*sizeof(float));
799 spanangle = MALLOC(float*,n*sizeof(float));
800 kVs = (struct SFRotation *)values;
801 nspan = n-1;
802 iend = n;
803 if(vecsame4f(kVs[0].c,kVs[n-1].c)) iend = n -1;
804 totalangle = 0.0;
805 k = 0;
806 if(0){
807 printf("key vals before:\n");
808 for(i=0;i<n;i++){
809 printf("%d %f %f %f %f %f\n",i,keys[i],values[i*4 +0],values[i*4 +1],values[i*4 +2],values[i*4 +3]);
810 }
811 }
812 for(i=0;i<nspan;i++){
813 Quaternion qi,qip1,qip2,qim1,si,sip1;
814 Quaternion qc, qfinal, qdiff;
815 double h;
816 int ip1, ip2, im1;
817 //i = ispan; //counter -1;
818 vrmlrot_to_quaternion (&qi, kVs[i].c[0], kVs[i].c[1], kVs[i].c[2], kVs[i].c[3]);
819 quaternion_normalize(&qi);
820 ip1 = i+1;
821 ip1 = closed ? iwrap(ip1,0,iend) : min(max(ip1,0),n-2);
822 vrmlrot_to_quaternion (&qip1, kVs[ip1].c[0],kVs[ip1].c[1], kVs[ip1].c[2], kVs[ip1].c[3]);
823 quaternion_normalize(&qip1);
824 ip2 =i+2;
825 ip2 = closed ? iwrap(ip2,0,iend) : min(max(ip2,0),n-1);
826 vrmlrot_to_quaternion (&qip2, kVs[ip2].c[0],kVs[ip2].c[1], kVs[ip2].c[2], kVs[ip2].c[3]);
827 quaternion_normalize(&qip2);
828 im1 = i-1;
829 im1 = closed ? iwrap(im1,0,iend) : min(max(im1,0),n-3);
830 vrmlrot_to_quaternion (&qim1, kVs[im1].c[0],kVs[im1].c[1], kVs[im1].c[2], kVs[im1].c[3]);
831 quaternion_normalize(&qim1);
832
833 if(k==0) qlast = qi;
834 //quaternion_squad_prepare(qim1,qi,qip1,qip2,s1,s2,q2);
835 //si
836 quaternion_squad_prepare(&qim1,&qi,&qip1,&qip2,&si,&sip1,&qc);
837
838 spanangle[i] = 0.0f;
839 for(j=0;j<10;j++){
840 float sfraction = j*.1f;
841
842 h = (double) sfraction; //interval;
843
844 quaternion_squad(&qfinal,&qi,&qc,&si,&sip1,h);
845 quaternion_normalize(&qfinal);
846 quaternion_diff(&qdiff,&qfinal,&qlast);
847 quaternion_normalize(&qdiff);
848 angle = acos(fwclampd(qdiff.w,-1.0,1.0))*2.0;
849 spanangle[i] += (float)angle;
850 mkeys[k] = keys[i] + sfraction * (keys[i+1] - keys[i]);
851 mvals[k] = qfinal;
852 totalangle += angle;
853 cumangle[k] = (float)totalangle;
854 if(0) printf("i %d j %d angle %lf\n",i,j,angle);
855 qlast = qfinal;
856
857 }
858 }
859 if(1) printf("total angle=%lf\n",totalangle);
860 velocity = totalangle / (keys[n-1] - keys[0]);
861
862 //method 2 adjust span keys
863 nkey = *normkeys = MALLOC(float*, n*sizeof(float));
864 nvals = *normvals = MALLOC(float*,n*sizeof(struct SFRotation));
865 memcpy(*normkeys,keys,n*sizeof(float));
866 memcpy(*normvals,values,n*sizeof(struct SFRotation));
867 cumkey = 0.0f;
868 cumkey = nkey[0] = keys[0];
869 for(i=0;i<nspan;i++){
870 float newspanfraction = (float)(spanangle[i]/velocity);
871 nkey[i+1] = newspanfraction + cumkey;
872 cumkey += newspanfraction;
873 }
874 *nnorm = n;
875 if(0){
876 printf("key vals after:\n");
877 for(i=0;i<*nnorm;i++){
878 printf("%d %f %f %f %f %f\n",i,nkey[i],nvals[i*4 +0],nvals[i*4 +1],nvals[i*4 +2],nvals[i*4 +3]);
879 }
880 printf("\n");
881 }
882}
883
884void do_SquadOrientationInterpolator(void *node){
885 // http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/interp.html#SquadOrientationInterpolator
886 // EXCEPT: specs seem to have a few things wrong
887 // 1. there's no 'velocity' stuff needed
888 // 2. no relation to the spine interpolators
889 // Squad research:
890 // https://msdn.microsoft.com/en-us/library/windows/desktop/bb281656(v=vs.85).aspx
891 // - Squad interp using slerps
892 // Slerp(Slerp(q1, c, t), Slerp(a, b, t), 2t(1 - t))
893 // https://theory.org/software/qfa/writeup/node12.html
894 // - Also squad via slerp
895 // squad(b0, S1, S2, b3, u) = slerp( slerp(b0,b3,u), slerp(S1, S2, u), 2u(1-u))
896 // http://run.usc.edu/cs520-s13/assign2/p245-shoemake.pdf
897 // - SHOE paper refered to by web3d specs, no mention of squad, uses bezier slerps.
898 // http://web.mit.edu/2.998/www/QuaternionReport1.pdf
899 // - page 51 explains SHOE
900 // qinterp = Squad(qi, qi+1,si,si+1,h) = Slerp( Slerp(qi,qi+1,h), Slerp(si,si+1,h), 2h(1-h))
901 // where si = qi * exp( - (log(inv(qi)*qi+1) + log(inv(qi)*qi-1)) / 4 )
902 // and qi+1 means q[i+1] and qi-1 means q[i-1] ie that's an indexer, not a quat + scalar
903 // p. 15 shows log(q) if q=[cost,sint*v] then
904 // log(q) = [0, sint*v] (non-unit)
905 // exp(q) = [cost, sint*v] (puts cos(t) back in scalar part, inverting log)
906 // remember from trig cos**2 + sin**2 = 1, or cos = sqrt(1 - sin**2)
907 // can probably take sine from sint*v ie sint = veclength(sint*v)
908 // http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
909 // p.10 "the logarithm of a unit quaternion q = [vˆ sin O, cos O] is just vˆO, a pure vector of length O."
910 // July 19, 2016 - code below copied from orientationInterpolator and node caste changed, but no other changes
911 // Dec 25, 2016 - squad coded
912
913
915 int kin, kvin;
916 struct SFRotation *kVs;
917 float *keys;
918 int iend;
919 //float interval; /* where we are between 2 values */
920 // UNUSED?? int stzero;
921 // UNUSED?? int endzero; /* starting and/or ending angles zero? */
922
923 Quaternion qfinal;
924 double x,y,z,a;
925
926 if (!node) return;
927 px = (struct X3D_SquadOrientationInterpolator *) node;
928 keys = px->key.p;
929 kin = ((px->key).n);
930 kvin = ((px->keyValue).n);
931 kVs = ((px->keyValue).p);
932
933 if(px->normalizeVelocity){
934 if(!px->_normkey.n){
935 float *normkeys, *normvals;
936 int nnorm;
937 squad_compute_velocity_normalized_key_keyvalue(px->closed,kin,keys,(float*)kVs,&nnorm,&normkeys,&normvals);
938 px->_normkey.n = nnorm;
939 px->_normkey.p = normkeys;
940 px->_normkeyValue.p = (struct SFRotation*)normvals;
941 px->_normkeyValue.n = nnorm;
942 }
943 kin = px->_normkey.n;
944 kvin = kin;
945 keys = px->_normkey.p;
946 kVs = px->_normkeyValue.p;
947 }
948
949 #ifdef SEVERBOSE
950 printf ("starting do_Oint4; keyValue count %d and key count %d\n",
951 kvin, kin);
952 #endif
953 if(0) debug_SquadOrientationInterpolator(px);
954
955 MARK_EVENT (node, offsetof (struct X3D_SquadOrientationInterpolator, value_changed));
956
957 /* make sure we have the keys and keyValues */
958 if ((kvin == 0) || (kin == 0)) {
959 px->value_changed.c[0] = (float) 0.0;
960 px->value_changed.c[1] = (float) 0.0;
961 px->value_changed.c[2] = (float) 0.0;
962 px->value_changed.c[3] = (float) 0.0;
963 return;
964 }
965 if (kin>kvin) kin=kvin; /* means we don't use whole of keyValue, but... */
966 //for wrap-around if closed, adjust the wrap around end value based on
967 //whether the scene author duplicated the first keyvalue in the last.
968 iend = kin;
969 if(vecsame4f(kVs[0].c,kVs[kin-1].c)) iend = kin -1;
970
971
972 /* set_fraction less than or greater than keys */
973 if (px->set_fraction <= keys[0]) {
974 memcpy ((void *)&px->value_changed,
975 (void *)&kVs[0], sizeof (struct SFRotation));
976 } else if (px->set_fraction >= keys[kin-1]) {
977 memcpy ((void *)&px->value_changed,
978 (void *)&kVs[kvin-1], sizeof (struct SFRotation));
979 } else {
980 int ispan;
981 float sfraction;
982 Quaternion qi,qip1,qip2,qim1,si,sip1;
983 Quaternion qc;
984 double h;
985 int ip1, ip2, im1, i;
986
987 ispan = find_key(kin,(float)(px->set_fraction),keys) -1;
988 //interval = (px->set_fraction - keys[counter-1]) /
989 // (keys[counter] - keys[counter-1]);
990 sfraction = span_fraction(px->set_fraction,keys[ispan],keys[ispan+1]);
991
992 /* are either the starting or ending angles zero? */
993 // unused? stzero = APPROX(kVs[counter-1].c[3],0.0);
994 // unused? endzero = APPROX(kVs[counter].c[3],0.0);
995 #ifdef SEVERBOSE
996 printf ("counter %d interval %f\n",counter,interval);
997 printf ("angles %f %f %f %f, %f %f %f %f\n",
998 kVs[counter-1].c[0],
999 kVs[counter-1].c[1],
1000 kVs[counter-1].c[2],
1001 kVs[counter-1].c[3],
1002 kVs[counter].c[0],
1003 kVs[counter].c[1],
1004 kVs[counter].c[2],
1005 kVs[counter].c[3]);
1006 #endif
1007 //squad
1008 // qinterp = Squad(qi, qi+1,si,si+1,h) = Slerp( Slerp(qi,qi+1,h), Slerp(si,si+1,h), 2h(1-h))
1009 //in theory, the vrmlrot_to_quaternion and compute_si can be done in a compile_squadorientationinterpolator step
1010 //then just quaternion_slerps here
1011 i = ispan; //counter -1;
1012 vrmlrot_to_quaternion (&qi, kVs[i].c[0], kVs[i].c[1], kVs[i].c[2], kVs[i].c[3]);
1013 quaternion_normalize(&qi);
1014 ip1 = i+1;
1015 ip1 = px->closed ? iwrap(ip1,0,iend) : min(max(ip1,0),kin-2);
1016 vrmlrot_to_quaternion (&qip1, kVs[ip1].c[0],kVs[ip1].c[1], kVs[ip1].c[2], kVs[ip1].c[3]);
1017 quaternion_normalize(&qip1);
1018 ip2 =i+2;
1019 ip2 = px->closed ? iwrap(ip2,0,iend) : min(max(ip2,0),kin-1);
1020 vrmlrot_to_quaternion (&qip2, kVs[ip2].c[0],kVs[ip2].c[1], kVs[ip2].c[2], kVs[ip2].c[3]);
1021 quaternion_normalize(&qip2);
1022 im1 = i-1;
1023 im1 = px->closed ? iwrap(im1,0,iend) : min(max(im1,0),kin-3);
1024 vrmlrot_to_quaternion (&qim1, kVs[im1].c[0],kVs[im1].c[1], kVs[im1].c[2], kVs[im1].c[3]);
1025 quaternion_normalize(&qim1);
1026
1027 //quaternion_squad_prepare(qim1,qi,qip1,qip2,s1,s2,q2);
1028 //si
1029 //compute_si(&si,&qi, &qip1, &qim1);
1030 //compute_si(&sip1,&qip1,&qip2,&qi);
1031 h = (double) sfraction; //interval;
1032
1033
1034 quaternion_squad_prepare(&qim1,&qi,&qip1,&qip2,&si,&sip1,&qc);
1035 quaternion_squad(&qfinal,&qi,&qc,&si,&sip1,h);
1036 quaternion_normalize(&qfinal);
1037 quaternion_to_vrmlrot(&qfinal,&x, &y, &z, &a);
1038 px->value_changed.c[0] = (float) x;
1039 px->value_changed.c[1] = (float) y;
1040 px->value_changed.c[2] = (float) z;
1041 px->value_changed.c[3] = (float) a;
1042
1043 #ifdef SEVERBOSE
1044 printf ("Oint, new angle %f %f %f %f\n",px->value_changed.c[0],
1045 px->value_changed.c[1],px->value_changed.c[2], px->value_changed.c[3]);
1046 #endif
1047 }
1048
1049}
1050
1051//END MIT LIC <<<<<<<
Definition Viewer.h:141