34#include <libFreeWRL.h>
37#include "../vrml_parser/Structs.h"
38#include "../input/InputFunctions.h"
39#include "../opengl/LoadTextures.h"
42#include "../main/headers.h"
43#include "../opengl/OpenGL_Utils.h"
44#include "../scenegraph/RenderFuncs.h"
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"
54#include "../input/SensInterps.h"
60#define NNC(A) NNC0(X3D_NODE(A))
61#define MNC(A) MNC0(X3D_NODE(A))
68void do_EaseInEaseOut(
void *node){
76 float fraction, u, eout, ein, S;
85 fraction = min(px->set_fraction,px->key.p[kin-1]);
86 fraction = max(px->set_fraction,px->key.p[0]);
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];
91 ein = px->easeInEaseOut.p[ispan+1].c[0];
95 px->modifiedFraction_changed = u;
102 t = 1.0f / (2.0f - eout - ein);
104 px->modifiedFraction_changed = (t/eout) * u*u;
105 }
else if(u < 1.0f - ein){
106 px->modifiedFraction_changed = (t*(2*u - eout));
108 px->modifiedFraction_changed = 1.0f - (t*(1.0f - u)*(1.0f - u))/ein;
136static void spline_interp(
int dim,
float *result,
float s,
float *val0,
float *val1,
float *T00,
float *T11){
140 float S[4], *C[4], SH[4];
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};
153 vecmultmat4f(SH,S,H);
163 result[i] += SH[j]*C[j][i];
169static void compute_spline_velocity_Ti(
int dim,
int normalize,
int nval,
float *val,
int nvel,
float* vel,
float *Ti ){
171 if(nvel && vel && nvel == nval){
176 memcpy(Ti,vel,dim*nval*
sizeof(
float));
184 for(i=0;i<nval-1;i++){
188 Di += (val[i+1] - val[i])*(val[i+1] - val[i]);
190 Dtot += (float)sqrt(Di);
196 veli = veli + (vel[i*dim + j]*vel[i*dim + j]);
197 veli = (float)sqrt(veli);
200 Ti[i*dim + j] = Dtot * vel[i*dim + j] / veli;
202 Ti[i*dim + j] = 0.0f;
213 Ti[ 0*dim + j] = vel[0*dim + j];
214 Ti[(nval-1)*dim + j] = vel[1*dim + j];
220 Ti[ 0*dim + j] = 0.0f;
221 Ti[(nval-1)*dim + j] = 0.0f;
225 for(i=1;i<nval-1;i++){
228 Ti[i*dim + j] = (val[(i+1)*dim +j] - val[(i-1)*dim +j]) * .5f;
233int iwrap(
int i,
int istart,
int iend){
240 if(iret < istart) iret = iend - (istart - iret);
241 if(iret >= iend ) iret = istart + (iret - iend);
244static void spline_velocity_adjust_for_keyspan(
int dim,
int closed,
int nval,
float *
key,
float *Ti,
float* T0,
float *T1){
247 int istart, iend, jend,i,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];
263 for(i=istart;i<iend;i++){
265 ip = iwrap(i+1,0,jend);
266 im = iwrap(i-1,0,jend);
270 T0[i*dim +j] = Fp*Ti[i*dim +j];
271 T1[i*dim +j] = Fm*Ti[i*dim +j];
276static float span_fraction(
float t,
float t0,
float t1){
278 ret = (t - t0) /(t1 - t0);
282void do_SplinePositionInterpolator(
void *node){
287 float fraction, sfraction;
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));
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;
309 px->_T1.p = (
struct SFVec3f*)T1;
319 kvin = px->keyValue.n;
324 if ((kvin == 0) || (kin == 0)) {
325 vecset3f(px->value_changed.c,0.0f,0.0f,0.0f);
328 if (kin>kvin) kin=kvin;
331 printf (
"ScalarInterpolator, kin %d kvin %d, vc %f\n",kin,kvin,px->value_changed);
335 fraction = min(px->set_fraction,px->key.p[kin-1]);
336 fraction = max(px->set_fraction,px->key.p[0]);
338 ispan =find_key(kin,fraction,px->key.p) -1;
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);
353void do_SplinePositionInterpolator2D(
void *node){
357 float fraction,sfraction;
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));
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;
379 px->_T1.p = (
struct SFVec2f*)T1;
389 kvin = px->keyValue.n;
394 if ((kvin == 0) || (kin == 0)) {
395 vecset2f(px->value_changed.c,0.0f,0.0f);
398 if (kin>kvin) kin=kvin;
401 printf (
"ScalarInterpolator, kin %d kvin %d, vc %f\n",kin,kvin,px->value_changed);
405 fraction = min(px->set_fraction,px->key.p[kin-1]);
406 fraction = max(px->set_fraction,px->key.p[0]);
408 ispan =find_key(kin,fraction,px->key.p) -1;
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);
424void do_SplineScalarInterpolator(
void *node){
430 float fraction, sfraction;
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");
448 T0 = MALLOC(
float*,n*dim*
sizeof(
float));
449 T1 = MALLOC(
float*,n*dim*
sizeof(
float));
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);
456 FREE_IF_NZ(px->_T0.p);
457 FREE_IF_NZ(px->_T1.p);
470 kvin = px->keyValue.n;
475 if ((kvin == 0) || (kin == 0)) {
476 px->value_changed = 0.0;
479 if (kin>kvin) kin=kvin;
482 printf (
"ScalarInterpolator, kin %d kvin %d, vc %f\n",kin,kvin,px->value_changed);
486 fraction = min(px->set_fraction,px->key.p[kin-1]);
487 fraction = max(px->set_fraction,px->key.p[0]);
489 ispan =find_key(kin,fraction,px->key.p) -1;
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]);
508static double *quaternion2double(
double *xyzw,
const Quaternion *q){
515static Quaternion *double2quaternion(Quaternion *q,
const double* xyzw){
522static void quaternion_addition(Quaternion *ret,
const Quaternion *q1,
const Quaternion *q2){
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);
532static void quaternion_subtraction(Quaternion *ret,
const Quaternion *q1,
const Quaternion *q2){
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);
541static double fwclampd(
const double val,
const double low,
const double hi){
547static double quaternion_dot(
const Quaternion *q1,
const Quaternion *q2){
548 double xyzw1[4], xyzw2[4], dot;
550 quaternion2double(xyzw1,q1);
551 quaternion2double(xyzw2,q1);
554 dot += xyzw1[i]*xyzw2[i];
557static void quaternion_negate(Quaternion *q){
560 quaternion2double(xyzw,q);
563 double2quaternion(q,xyzw);
565static void quaternion_log(Quaternion *ret,
const Quaternion *q){
566 double angle, angle_over_sine, xyzw[4];
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);
574 double2quaternion(ret,xyzw);
576static void quaternion_exp(Quaternion *ret,
const Quaternion *q){
577 double angle, xyzw[4], sine_over_angle;
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);
587static void compute_si(Quaternion *si,Quaternion *qi,
const Quaternion *qip1,
const Quaternion *qim1){
600 Quaternion qiinv, qiinv_qip1, qiinv_qim1,qadded,qexp;
603 quaternion_inverse(&qiinv,qi);
604 quaternion_multiply(&qiinv_qip1,&qiinv,qip1);
606 quaternion_log(&qiinv_qip1,&qiinv_qip1);
607 quaternion_multiply(&qiinv_qim1,&qiinv,qim1);
608 quaternion_log(&qiinv_qim1,&qiinv_qim1);
610 quaternion_addition(&qadded,&qiinv_qip1,&qiinv_qim1);
611 quaternion2double(xyzw,&qadded);
612 vecscaled(xyzw,xyzw,-.25);
616 double2quaternion(&qexp,xyzw);
617 quaternion_exp(&qexp,&qexp);
620 quaternion_multiply(si,qi,&qexp);
629 Quaternion qi,qip1,qip2,qim1,si,sip1;
630 int ip1, ip2, im1, kin, iend,i;
632 static int once_debug = 0;
633 if(once_debug != 0)
return;
640 if(vecsame4f(px->keyValue.p[0].c,px->keyValue.p[kin-1].c)) iend = kin -1;
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]);
653 vrmlrot_to_quaternion (&qi, kVs[i].c[0], kVs[i].c[1], kVs[i].c[2], kVs[i].c[3]);
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]);
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]);
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]);
666 compute_si(&si,&qi, &qip1, &qim1);
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);
675 quaternion_to_vrmlrot(&si,&xyza[0],&xyza[1],&xyza[2],&xyza[3] );
684static void quaternion_lerp(Quaternion *ret,
const Quaternion *q1,
const Quaternion *q2,
const double t){
688 quaternion_scalar_multiply(&t2,t);
689 quaternion_scalar_multiply(&t1,1.0 -t);
690 quaternion_addition(ret,&t1,&t2);
692static void quaternion_slerp2(Quaternion *ret,
const Quaternion *q1,
const Quaternion *q2,
const double t){
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);
703 quaternion_negate(&r);
705 if(1.0 - dot < .001){
706 quaternion_lerp(ret,q1,&r,t);
709 double angle = acos(dot);
712 quaternion_scalar_multiply(&t1,sin((1.0-t)*angle));
713 quaternion_scalar_multiply(&t2,sin(t*angle));
714 quaternion_addition(ret,&t1,&t2);
716 quaternion_scalar_multiply(ret,1.0/sin(angle));
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;
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);
744 compute_si(s1,&q1,&q2,&q0);
745 compute_si(s2,&q2,&q3,&q1);
749static void quaternion_squad(Quaternion *
final,Quaternion *q1,Quaternion *q2, Quaternion *s1,Quaternion *s2,
double t){
751 quaternion_slerp2(&qs,q1,q2,t);
752 quaternion_slerp2(&ss,s1,s2,t);
753 quaternion_slerp2(
final, &qs, &ss, 2.0*t*(1.0 -t));
754 quaternion_normalize(
final);
756static void quaternion_diff(Quaternion *qdiff, Quaternion *q2, Quaternion *q1){
760 quaternion_inverse(&q1inv,q1);
761 quaternion_multiply(qdiff,q2,&q1inv);
763 quaternion_negate(&q1inv);
764 quaternion_multiply(qdiff,q2,&q1inv);
768static void squad_compute_velocity_normalized_key_keyvalue(
int closed,
769 int n,
float *keys,
float *values,
770 int *nnorm,
float **normkeys,
float **normvals)
790 Quaternion qlast, *mvals;
791 double totalangle, angle, velocity;
792 int i,j,k, iend, nspan;
793 float *mkeys,*cumangle, *spanangle, *nkey, *nvals, cumkey;
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));
803 if(vecsame4f(kVs[0].c,kVs[n-1].c)) iend = n -1;
807 printf(
"key vals before:\n");
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]);
812 for(i=0;i<nspan;i++){
813 Quaternion qi,qip1,qip2,qim1,si,sip1;
814 Quaternion qc, qfinal, qdiff;
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);
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);
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);
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);
836 quaternion_squad_prepare(&qim1,&qi,&qip1,&qip2,&si,&sip1,&qc);
840 float sfraction = j*.1f;
842 h = (double) sfraction;
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]);
853 cumangle[k] = (float)totalangle;
854 if(0) printf(
"i %d j %d angle %lf\n",i,j,angle);
859 if(1) printf(
"total angle=%lf\n",totalangle);
860 velocity = totalangle / (keys[n-1] - keys[0]);
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));
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;
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]);
884void do_SquadOrientationInterpolator(
void *node){
930 kvin = ((px->keyValue).n);
931 kVs = ((px->keyValue).p);
933 if(px->normalizeVelocity){
935 float *normkeys, *normvals;
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;
943 kin = px->_normkey.n;
945 keys = px->_normkey.p;
946 kVs = px->_normkeyValue.p;
950 printf (
"starting do_Oint4; keyValue count %d and key count %d\n",
953 if(0) debug_SquadOrientationInterpolator(px);
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;
965 if (kin>kvin) kin=kvin;
969 if(vecsame4f(kVs[0].c,kVs[kin-1].c)) iend = kin -1;
973 if (px->set_fraction <= keys[0]) {
974 memcpy ((
void *)&px->value_changed,
976 }
else if (px->set_fraction >= keys[kin-1]) {
977 memcpy ((
void *)&px->value_changed,
978 (
void *)&kVs[kvin-1], sizeof (
struct SFRotation));
982 Quaternion qi,qip1,qip2,qim1,si,sip1;
985 int ip1, ip2, im1, i;
987 ispan = find_key(kin,(
float)(px->set_fraction),keys) -1;
990 sfraction = span_fraction(px->set_fraction,keys[ispan],keys[ispan+1]);
996 printf (
"counter %d interval %f\n",counter,interval);
997 printf (
"angles %f %f %f %f, %f %f %f %f\n",
1000 kVs[counter-1].c[2],
1001 kVs[counter-1].c[3],
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);
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);
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);
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);
1031 h = (double) sfraction;
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;
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]);