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