自学教程：C++ vcross函数代码示例

//perhaps in engine is better? void find_best_split_plane( const std::vector <face_t >& faces							// ,std::vector <face_t >& front							// ,std::vector <face_t >& back	) {	//all time low	score_t low={},s={};	low.score = 999999;			//along planes, along edges...	for ( size_t i = faces.size() ; i--; )	{		face_t f = faces[i];		//xplane_t p = { f.a, f.normal() };		//test_plane(p, faces, s);		//enum{ABC,AB,BC,CA};		vec3 N = f.normal();		plane_t planes[4] = {			pmake( f.a, N),			//this should always end up on front/front_on side			pmake( f.a, vnormal(vcross( N, f.b - f.a ) ) ),			pmake( f.b, vnormal(vcross( N, f.c - f.b ) ) ),			pmake( f.c, vnormal(vcross( N, f.a - f.c ) ) )		};		//test planes		for ( int j = 0; j < 4 ; ++j )		{			test_plane( planes[j], faces, s);			if( s.score < low.score )				low=s,low.face=i;		}	}	//check score	//use low.p to split front and backm or stop here if bad score		//print	printf( "/n/nBest plane for all %d/n", faces.size());	printf( "index %d/n", low.face);	printf( "score = %d/n", low.score );	printf( "balance = %d/n", low.balance );	printf( "splits = %d/n", low.split );	printf( "coins = %d/n", low.coin );	printf( "front = %d/n", low.front );	printf( "back = %d/n", low.back );	printf( "plane = %f, %f, %f,   %f/n",			low.p.x,low.p.y,low.p.z,low.p.w );}

/* * Find a intersection point for each beam's corner ray onto the triangle * plane. * Points found may not lie within the triangle. */intfind_isect_pos_onto_the_triangle_plane(    ri_vector_t   *points,      /* [out]    */    ri_beam_t     *beam,    ri_triangle_t *triangle){    int         i;    ri_vector_t v0, v1, v2;     ri_vector_t e1, e2;     ri_vector_t p, s, q;    ri_float_t  a, inva;    ri_float_t  t;    double      eps = 1.0e-14;    vcpy( v0, triangle->v[0] );    vcpy( v1, triangle->v[1] );    vcpy( v2, triangle->v[2] );    vsub( e1, v1, v0 );    vsub( e2, v2, v0 );    vsub( s, beam->org, v0 );    for (i = 0; i < 4; i++) {        vcross( p, beam->dir[i], e2 );        a = vdot( e1, p );        if (fabs(a) > eps) {            inva = 1.0 / a;        } else {            inva = 1.0;        }        vcross( q, s, e1 );        t = vdot( e2, q ) * inva;                    points[i][0] = beam->org[0] + t * beam->dir[i][0];        points[i][1] = beam->org[1] + t * beam->dir[i][1];        points[i][2] = beam->org[2] + t * beam->dir[i][2];    }    return 0;}

void add_quats(double q1[4], double q2[4], double dest[4]){   static int count=0;   double t1[4], t2[4], t3[4];   double tf[4];   vcopy(t1, q1);   vscale(t1,q2[3]);   vcopy(t2, q2);   vscale(t2,q1[3]);   vcross(t3,q2,q1);   vadd(tf,t1,t2);   vadd(tf,t3,tf);   tf[3] = q1[3] * q2[3] - vdot(q1,q2);   dest[0] = tf[0];   dest[1] = tf[1];   dest[2] = tf[2];   dest[3] = tf[3];   if (++count > RENORMCOUNT) {      count = 0;      normalize_quat(dest);      }   }

voidadd_quats(float q1[4], float q2[4], float dest[4]){    static int count=0;    float t1[4], t2[4], t3[4];    float tf[4];    vcopy(q1,t1);    vscale(t1,q2[3]);    vcopy(q2,t2);    vscale(t2,q1[3]);    vcross(q2,q1,t3);    vadd(t1,t2,tf);    vadd(t3,tf,tf);    tf[3] = q1[3] * q2[3] - vdot(q1,q2);    dest[0] = tf[0];    dest[1] = tf[1];    dest[2] = tf[2];    dest[3] = tf[3];    if (++count > RENORMCOUNT) {        count = 0;        normalize_quat(dest);    }}

//save loadvec3 uv_tangent(vec3 v0,vec3 v1,vec3 uv0,vec3 uv1){//	if(vdot(uv0,uv1)==0.f)	if(uv0.x==uv1.x && uv0.y==uv1.y)	{		printf("WARNING!!! Zero area UV./n");		//set to face tangent		return vnormal( vcross( v0, v1 ) );	}else{		float coef = 1./(uv0.x * uv1.y - uv1.x * uv0.y);		vec3 t={			coef * ((v0.x * uv1.y)  + (v1.x * -uv0.y)),			coef * ((v0.y * uv1.y)  + (v1.y * -uv0.y)),			coef * ((v0.z * uv1.y)  + (v1.z * -uv0.y))		};		float len=vlen(t);		if(!len){			printf("ERROR: zero area UV  @uv_tangent./n");			exit(-1);		}		t/=len;				return t;	}}

/* * Ok, simulate a track-ball.  Project the points onto the virtual * trackball, then figure out the axis of rotation, which is the cross * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0) * Note:  This is a deformed trackball-- is a trackball in the center, * but is deformed into a hyperbolic sheet of rotation away from the * center.  This particular function was chosen after trying out * several variations. * * It is assumed that the arguments to this routine are in the range * (-1.0 ... 1.0) */static voidtrackball ( float q[4], float p1x, float p1y, float p2x, float p2y ){    float a[3]; /* Axis of rotation */    float phi;  /* how much to rotate about axis */    float p1[3], p2[3], d[3];    float t;    if (p1x == p2x && p1y == p2y)    {        // Zero rotation        vzero(q);        q[3] = 1.0;        return;    }    // First, figure out z-coordinates for projection of P1 and P2 to    // deformed sphere    vset(p1,p1x,p1y,tb_project_to_sphere(TRACKBALLSIZE,p1x,p1y));    vset(p2,p2x,p2y,tb_project_to_sphere(TRACKBALLSIZE,p2x,p2y));    // Now, we want the cross product of P1 and P2    vcross(p2,p1,a);    // Figure out how much to rotate around that axis.    vsub(p1,p2,d);    t = vlength(d) / (2.0*TRACKBALLSIZE);    // Avoid problems with out-of-control values...    if (t > 1.0) t = 1.0;    if (t < -1.0) t = -1.0;    phi = 2.0 * asin(t);    axis_to_quat(a,phi,q);}

void add_eulers(float *e1, float *e2, float *dest){	static int count=0;	register int i;	float t1[3], t2[3], t3[3];	float tf[4];		vcopy(e1, t1); vscale(t1, e2[3]);	vcopy(e2, t2); vscale(t2, e1[3]);	vcross(e2, e1, t3);	vadd(t1, t2, tf);	vadd(t3, tf, tf);	tf[3] = e1[3] * e2[3] - vdot(e1, e2);		for (i = 0 ; i < 4 ;i++)	{		dest[i] = tf[i];	}		if (++count > COUNT)	{		count = 0;		normalize_euler(dest);	}}

/* returns the rotation quat required to match v1 to v2    */void vects_to_quat(double v1[3], double v2[3], double q[4]){   double axis[3];   double phi;   double denom;      /* first normalise each of em */   vnormal(v1);   vnormal(v2);      /* check that someone isn't playing silly buggers */   if((v1[0] == v2[0]) && (v1[1] == v2[1]) && (v1[2] == v2[2])){      vcopy(axis, v1);      phi = 0.0;      }   else{      /* find the axis with a cross product          */      vcross(axis, v2, v1);      /* find the angle to rotate around that axis.  */      /* v1.v2 = ||v1|| ||v2|| cos(angle)            */      denom = vlength(v1) * vlength(v2);      if(denom == 0){         phi = 0.0;         }      else{         phi = acos(vdot(v1, v2) / denom);         }      }      /* create the quat */   axis_to_quat(axis, phi, q);   }

void Trackball :: spin ( float friction ){    const int RENORMCOUNT = 97 ;    static int count=0;    //float * q1, * q2, * dest ;    float q1[4], * q2, * dest ;    float t1[4], t2[4], t3[4];    float tf[4];    //q1 = m_lastquat ;    if( friction != 1.0 )    {      float temp ;      temp = vlength(m_lastquat);      if( temp != 0 )      {         vcopy(m_lastquat,q1);         vnormal(q1) ;         temp = asin(temp)*friction ;         q1[0] *= sin(temp) ;         q1[1] *= sin(temp) ;         q1[2] *= sin(temp) ;         q1[3] = cos(temp) ;      }      else      {         vcopy(m_lastquat,q1);         q1[3] = m_lastquat[3] ;      }    }    else    {        vcopy(m_lastquat,q1);        q1[3] = m_lastquat[3] ;    }    q2 = m_currquat ;    dest = m_currquat ;    vcopy(q1,t1);    vscale(t1,q2[3]);    vcopy(q2,t2);    vscale(t2,q1[3]);    vcross(q2,q1,t3);    vadd(t1,t2,tf);    vadd(t3,tf,tf);    tf[3] = q1[3] * q2[3] - vdot(q1,q2);    dest[0] = tf[0];    dest[1] = tf[1];    dest[2] = tf[2];    dest[3] = tf[3];    if (++count > RENORMCOUNT) {        count = 0;        normalize_quat(dest);    }}

static void calcTriNormal(const float* v0, const float* v1, const float* v2, float* norm){	float e0[3], e1[3];	vsub(e0, v1, v0);	vsub(e1, v2, v0);	vcross(norm, e0, e1);	vnormalize(norm);}

void		setup_camera_plane(t_env *e){	t_vector	n;	t_vector	c;	double		w;	double		h;	h = 18.0 * ARBITRARY_NUMBER / 35.0;	w = h * (double)e->x / (double)e->y;	n = vunit(vsub(e->camera.loc, e->camera.dir));	e->camera.u = vunit(vcross(e->camera.up, n));	e->camera.v = vunit(vcross(n, e->camera.u));	c = vsub(e->camera.loc, vmult(n, ARBITRARY_NUMBER));	e->camera.l = vadd(vsub(c,		vmult(e->camera.u, w / 2.0)),		vmult(e->camera.v, h / 2.0));	e->camera.stepx = w / (double)e->x;	e->camera.stepy = h / (double)e->y;}

void appendArrowHead(struct duDebugDraw* dd, const float* p, const float* q,					 const float s, unsigned int col){	if (!dd) return;	float ax[3], ay[3] = {0,1,0}, az[3];	vsub(az, q, p);	vnormalize(az);	vcross(ax, ay, az);	vcross(ay, az, ax);	vnormalize(ay);	dd->vertex(p, col);//	dd->vertex(p[0]+az[0]*s+ay[0]*s/2, p[1]+az[1]*s+ay[1]*s/2, p[2]+az[2]*s+ay[2]*s/2, col);	dd->vertex(p[0]+az[0]*s+ax[0]*s/3, p[1]+az[1]*s+ax[1]*s/3, p[2]+az[2]*s+ax[2]*s/3, col);	dd->vertex(p, col);//	dd->vertex(p[0]+az[0]*s-ay[0]*s/2, p[1]+az[1]*s-ay[1]*s/2, p[2]+az[2]*s-ay[2]*s/2, col);	dd->vertex(p[0]+az[0]*s-ax[0]*s/3, p[1]+az[1]*s-ax[1]*s/3, p[2]+az[2]*s-ax[2]*s/3, col);	}

void PhysArbiter::PreStep(float inv_dt){	const float allowedPenetration = 0.01f;	float biasFactor = 0.2;	for(int i=0;i<numContacts;++i)	{		PhysContact *c = &contacts[i];		float2 r1 = c->pos - body1->pos;		float2 r2 = c->pos - body2->pos;				float rn1 = r1.dot(c->normal);		float rn2 = r2.dot(c->normal);		float kNormal = body1->invMass + body2->invMass;		kNormal += body1->invInertia * (r1.dot(r1) - (rn1*rn1)) +			body2->invInertia * (r2.dot(r2) - (rn2*rn2));		c->massNormal = 1.0f / kNormal;		float2 tangent = vcross(c->normal, 1.0f);		float rt1 = r1.dot(tangent);		float rt2 = r2.dot(tangent);		float kTangent = body1->invMass + body2->invMass;		kTangent += body1->invInertia * (r1.dot(r1) - rt1 * rt1) +			body2->invInertia * (r2.dot(r2) - rt2 * rt2);		c->massTangent = 1.0f/kTangent;		//c->bias = -biasFactor * invDt * min(0.0f, c->seperation + allowedPenetration);		/*float2 relativeVelocity = body2->velocity + (vcross(r2, body2->angularVelocity));		relativeVelocity -= body1->velocity - (vcross(r1, body1->angularVelocity));		float combinedRestitution = body1->restitution * body2->restitution;		float relVel = c->normal.dot(relativeVelocity);		c->restitution = max(combinedRestitution * -relVel, 0.0f);		float penVel = -c->seperation / dt;		c->bias = c->restitution>=penVel ? 0 : -biasFactor * invDt * min(0.0f, c->seperation + allowedPenetration);*/		c->bias = -biasFactor * inv_dt * min(0.0f, c->seperation+allowedPenetration);		// Accumulate impulses		/*float2 P = (c->normal*c->accNormalImpulse) +			(tangent*c->accTangentImpulse);		body1->velocity -= P.mul(body1->invMass);		body1->angularVelocity -= body1->invInertia * vcross(r1, P);		body2->velocity += P.mul(body2->invMass);		body2->angularVelocity += body2->invInertia * vcross(r2, P);*/		// End impulse accumulation		c->biasImpulse = 0;	}};

static inline voidorthoBasis(vec basis[3], const vec &n) {    basis[2] = n;    basis[1].x = 0.0; basis[1].y = 0.0; basis[1].z = 0.0;    if ((n.x < 0.6f) && (n.x > -0.6f)) {        basis[1].x = 1.0;    } else if ((n.y < 0.6f) && (n.y > -0.6f)) {        basis[1].y = 1.0;    } else if ((n.z < 0.6f) && (n.z > -0.6f)) {        basis[1].z = 1.0;    } else {        basis[1].x = 1.0;    }    basis[0] = vcross(basis[1], basis[2]);    vnormalize(basis[0]);    basis[1] = vcross(basis[2], basis[0]);    vnormalize(basis[1]);}

int main(int argc, char* argv[]){    puts("args: path/to/obj path/to/bmp");    FILE* const fobj =        oload(argc == 3 ? argv[1] : "model/salesman.obj");    SDL_Surface* const fdif =        sload(argc == 3 ? argv[2] : "model/salesman.bmp");    const Obj obj = oparse(fobj);    const Triangles tv = tvgen(obj); // Triangle Vertices.    const Triangles tt = ttgen(obj); // Triangle Textures.    const Triangles tn = tngen(obj); // Triangle Normals.    const Sdl sdl = ssetup(800, 600);    float* const zbuff = (float*) malloc(sizeof(float) * sdl.xres * sdl.yres);    for(Input input = iinit(); !input.done; input = ipump(input))    {        uint32_t* const pixel = slock(sdl);        reset(zbuff, pixel, sdl.xres * sdl.yres);        const Vertex center = { 0.0f, 0.0f, 0.0f };        const Vertex upward = { 0.0f, 1.0f, 0.0f };        const Vertex eye = { sinf(input.xt), sinf(input.yt), cosf(input.xt) };        const Vertex z = vunit(vsub(eye, center));        const Vertex x = vunit(vcross(upward, z));        const Vertex y = vcross(z, x);        for(int i = 0; i < tv.count; i++)        {            const Triangle nrm = tviewnrm(tn.triangle[i], x, y, z);            const Triangle tex = tt.triangle[i];            const Triangle tri = tviewtri(tv.triangle[i], x, y, z, eye);            const Triangle per = tperspective(tri);            const Triangle vew = tviewport(per, sdl);            const Target target = { vew, nrm, tex, fdif };            tdraw(sdl.yres, pixel, zbuff, target);        }        sunlock(sdl);        schurn(sdl);        spresent(sdl);    }    // Let the OS free hoisted memory for a quick exit.    return 0;}

bool ray_triangle(    vec3 p , vec3 d, vec3 A,vec3 B, vec3 C){    //not matrix translate rotate?    vec3 N=vcross(B-A, C-A);    dassert(vlen(N)!=0.f && "RAY_TRIANGLE");//	bool backface = vdot( N, d ) > 0.f;	float t=vdot(A-p,N)/vdot(N,d);	    //too far away or zero    if(t>=1.||t<=0.f)        return 0;	/*		float va = vdot( N , vcross( B-p, C-p ) );	float vb = vdot( N , vcross( C-p, A-p ) );	float vc = vdot( N , vcross( A-p, B-p ) );	float u = va / ( va + vb + vc );	float v = vb / ( va + vb + vc );	float w = 1.f - u - v;	if( u<0.f || v < 0.f || w < 0.f )		return 0;*/	//try uv approach    // check if ray is inside triangle	float EPS = -.00001f;//000000001f;    if( //!backface &&         (vdot(vcross(A-p,B-p),d)>EPS||		 vdot(vcross(B-p,C-p),d)>EPS||	 	 vdot(vcross(C-p,A-p),d)>EPS))         return 0;//miss triangle    return 1;}

static voidorthoBasis(vec *basis, vec n){    basis[2] = n;    basis[1].x = 0.0; basis[1].y = 0.0; basis[1].z = 0.0;    if ((n.x < 0.6) && (n.x > -0.6)) {        basis[1].x = 1.0;    } else if ((n.y < 0.6) && (n.y > -0.6)) {        basis[1].y = 1.0;    } else if ((n.z < 0.6) && (n.z > -0.6)) {        basis[1].z = 1.0;    } else {        basis[1].x = 1.0;    }    vcross(&basis[0], basis[1], basis[2]);    vnormalize(&basis[0]);    vcross(&basis[1], basis[2], basis[0]);    vnormalize(&basis[1]);}

int sphere_refraction_func(const struct TObject *object, const Ray *ray, const Ray *reflect, const Vector *pt,                           const Vector *n, Ray *refract, Vector *attenuation) {  // 入射角i  const Sphere *sp = (const Sphere*)object->priv;  double cosi = dot(n, &ray->front) / modulation(n) / modulation(&ray->front);  double sini = sqrt(1 - cosi*cosi);  double sinr;  Vector vin = *n;  if (cosi < 0) {    // 空气到介质    sinr = sini / sp->refractive;  }  else {    // 介质到空气    sinr = sini * sp->refractive;    vin = rmul(n, -1);    // 全反射    if (sinr >= 1)      return 0;  }  double r = asin(sinr);  Vector left = vcross(vcross(*n, ray->front), *n);  normalize(&left);  Vector nn = *n;  normalize(&nn);  refract->front = vadd(vrmul(left, sin(r)), vrmul(nn, cos(r)));  refract->pos = *pt;  *attenuation = sp->refract_attenuation;  return 1;}

void qmul(quat *dest, quat *a, quat *b) {	vec tmp;		dest->s = a->s*b->s - vdot(a->v,b->v);		memcpy(dest->v, a->v, sizeof(vec));	vmul(dest->v, b->s);		memcpy(tmp, b->v, sizeof(vec));	vmul(tmp, a->s);		vadd(dest->v, tmp);		vcross(tmp, a->v, b->v);	vadd(dest->v, tmp);}

/* * Ok, simulate a track-ball.  Project the points onto the virtual * trackball, then figure out the axis of rotation, which is the cross * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0) * Note:  This is a deformed trackball-- is a trackball in the center, * but is deformed into a hyperbolic sheet of rotation away from the * center.  This particular function was chosen after trying out * several variations. * * It is assumed that the arguments to this routine are in the range * (-1.0 ... 1.0) */void trackball( double q[4], double p1x, double p1y, double p2x, double p2y ){    double a[3]; /* Axis of rotation */    double phi;  /* how much to rotate about axis */    double p1[3], p2[3], d[3];    double t;    if( p1x == p2x && p1y == p2y )    {        /* Zero rotation */        vzero( q );        q[3] = 1.0;        return;    }    /*     * First, figure out z-coordinates for projection of P1 and P2 to     * deformed sphere     */    vset( p1, p1x, p1y, tb_project_to_sphere( TRACKBALLSIZE, p1x, p1y ) );    vset( p2, p2x, p2y, tb_project_to_sphere( TRACKBALLSIZE, p2x, p2y ) );    /*     *  Now, we want the cross product of P1 and P2     */    vcross(p2,p1,a);    /*     *  Figure out how much to rotate around that axis.     */    vsub( p1, p2, d );    t = vlength( d ) / (2.0f * TRACKBALLSIZE);    /*     * Avoid problems with out-of-control values...     */    if( t > 1.0 )        t = 1.0;    if( t < -1.0 )        t = -1.0;    phi = 2.0f * (double) asin( t );    axis_to_quat( a, phi, q );}

/** Find orthogonal factor Q of rank 2 (or less) M using adjoint transpose **/void do_rank2(HMatrix M, HMatrix MadjT, HMatrix Q){    float v1[3], v2[3];    float w, x, y, z, c, s, d;    int col;    /* If rank(M) is 2, we should find a non-zero column in MadjT */    col = find_max_col(MadjT);    if (col<0) {        do_rank1(M, Q);    /* Rank<2 */        return;    }    v1[0] = MadjT[0][col];    v1[1] = MadjT[1][col];    v1[2] = MadjT[2][col];    make_reflector(v1, v1);    reflect_cols(M, v1);    vcross(M[0], M[1], v2);    make_reflector(v2, v2);    reflect_rows(M, v2);    w = M[0][0];    x = M[0][1];    y = M[1][0];    z = M[1][1];    if (w*z>x*y) {        c = z+w;        s = y-x;        d = static_cast<float>(sqrt(c*c+s*s));        c = c/d;        s = s/d;        Q[0][0] = Q[1][1] = c;        Q[0][1] = -(Q[1][0] = s);    } else {        c = z-w;        s = y+x;        d = static_cast<float>(sqrt(c*c+s*s));        c = c/d;        s = s/d;        Q[0][0] = -(Q[1][1] = c);        Q[0][1] = Q[1][0] = s;    }    Q[0][2] = Q[2][0] = Q[1][2] = Q[2][1] = 0.0;    Q[2][2] = 1.0;    reflect_cols(Q, v1);    reflect_rows(Q, v2);}

int main(int argc, char**argv){	vec3 a = {0,0,1},b={0,1,0},c;	c = vcross(b,a);	printf( "C = %f, %f, %f/n/n",c.x,c.y,c.z);	const char*out_file,*in_file;	//open up enter root element	if ( argc < 3 )		return printf( " Sorry, too few args./n" ),-1;    in_file = argv[1];	out_file = argv[2];    TiXmlDocument doc(in_file);    if(!doc.LoadFile())//doc.Error()==1)        return printf("ERR: %s/n",  doc.ErrorDesc()), -1;	TiXmlElement*root=doc.RootElement();	if(!root)		return printf("Root not found/n"),-1;	//MATERIAL NAMES 	//get_material_names(root);	//READ LIGHTS	get_lights(root);	//READ GEO (polys)	get_geometry(root);	//Convert to poly / vertex format	dae_geo_to_polys_and_verts( dae_geo_objects[0] );		//... nau... do lighting calculations (simple at first	render_vertex_colors();		convert();	write(out_file);	//	render();    return 0;}

void add_quats_no_renorm(float q1[4], float q2[4], float dest[4]){    float t1[4], t2[4], t3[4];    float tf[4];    vcopy(q1,t1);    vscale(t1,q2[3]);    vcopy(q2,t2);    vscale(t2,q1[3]);    vcross(q2,q1,t3);    vadd(t1,t2,tf);    vadd(t3,tf,tf);    tf[3] = q1[3] * q2[3] - vdot(q1,q2);    dest[0] = tf[0];    dest[1] = tf[1];    dest[2] = tf[2];    dest[3] = tf[3];}

voidadd_quats(float q1[4], float q2[4], float dest[4]){    static int count=0;    float t1[4], t2[4], t3[4];    float tf[4];#if 0printf("q1 = %f %f %f %f/n", q1[0], q1[1], q1[2], q1[3]);printf("q2 = %f %f %f %f/n", q2[0], q2[1], q2[2], q2[3]);#endif    vcopy(q1,t1);    vscale(t1,q2[3]);    vcopy(q2,t2);    vscale(t2,q1[3]);    vcross(q2,q1,t3);    vadd(t1,t2,tf);    vadd(t3,tf,tf);    tf[3] = q1[3] * q2[3] - vdot(q1,q2);#if 0printf("tf = %f %f %f %f/n", tf[0], tf[1], tf[2], tf[3]);#endif    dest[0] = tf[0];    dest[1] = tf[1];    dest[2] = tf[2];    dest[3] = tf[3];    if (++count > RENORMCOUNT) {        count = 0;        normalize_quat(dest);    }}

/* Ok, simulate a track-ball.  Project the points onto the virtual * trackball, then figure out the axis of rotation, which is the cross * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0) * Note:  This is a deformed trackball-- is a trackball in the center, * but is deformed into a hyperbolic sheet of rotation away from the * center.  This particular function was chosen after trying out * several variations. * * It is assumed that the arguments to this routine are in the range * (-1.0 ... 1.0) */void trackball(double q[4], double p1x, double p1y, double p2x, double p2y){   double axis[3];             /* Axis of rotation              */   double phi;                 /* how much to rotate about axis */   double p1[3], p2[3], d[3];   double t;   /* if zero rotation */   if(p1x == p2x && p1y == p2y){      vset(q, 0.0, 0.0, 0.0);      q[3] = 1.0;      return;      }   /* First, figure out z-coordinates for projection of P1 and P2 to  */   /* the deformed sphere                                                 */   p1[0] = p1x;   p1[1] = p1y;   p1[2] = tb_project_to_sphere(TRACKBALLSIZE, p1x, p1y);   p2[0] = p2x;   p2[1] = p2y;   p2[2] = tb_project_to_sphere(TRACKBALLSIZE, p2x, p2y);   /* Now, we want the cross product of P1 and P2     */   vcross(axis, p2, p1);   /* Figure out how much to rotate around that axis. */   vsub(d, p1, p2);   t = vlength(d)/(2.0*TRACKBALLSIZE);   /* Avoid problems with out-of-control values.      */   if(t >  1.0){ t =  1.0; }   if(t < -1.0){ t = -1.0; }   phi = 2.0 * asin(t);   axis_to_quat(axis, phi, q);   }

/* * Ok, simulate a track-ball.  Project the points onto the virtual * trackball, then figure out the axis of rotation, which is the cross * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0) * Note:  This is a deformed trackball-- is a trackball in the center, * but is deformed into a hyperbolic sheet of rotation away from the * center.  This particular function was chosen after trying out * several variations. * * It is assumed that the arguments to this routine are in the range * (-1.0 ... 1.0) */void trackball(float q[4], float p1x, float p1y, float p2x, float p2y,  float tbSize){    float a[3];		// Axis of rotation    float phi;		// how much to rotate about axis    float p1[3], p2[3], d[3];    float t;    if( p1x == p2x && p1y == p2y ) {        /* Zero rotation */        vzero(q);        q[3] = 1.0;        return;    }    // First, figure out z-coordinates for projection of P1 and P2 to deformed sphere    vset( p1, p1x, p1y, tb_project_to_sphere(tbSize, p1x, p1y) );    vset( p2, p2x, p2y, tb_project_to_sphere(tbSize, p2x, p2y) );    // Now, we want the cross product of P1 and P2    vcross( p2, p1, a);    // Figure out how much to rotate around that axis    vsub( p1, p2, d);    t = vlength(d) / ( 2.0f * tbSize );    // Avoid problems with out-of-control values    if (t > 1.0) {		t = 1.0;	}    if (t < -1.0) {		t = -1.0;	}    phi = 2.0f * (float)asin(t);    axis_to_quat( a, phi, q );}

//.........这里部分代码省略.........             */            fprintf(stderr, "TODO: Beam's dir does not have same sign./n");            for (j = 0; j < 4; j++) {                fprintf(stderr, "      dir[%d] = %f, %f, %f/n", j,                    dir[j][0], dir[j][1], dir[j][2]);            }            return -1;        }    }    vcpy( beam->org,    org    );    /*     * Find dominant plane. Use dir[0]     */    maxval        = fabs(dir[0][0]);    dominant_axis = 0;    if ( (maxval < fabs(dir[0][1])) ) {        maxval        = fabs(dir[0][0]);        dominant_axis = 1;    }    if ( (maxval < fabs(dir[0][2])) ) {        maxval        = fabs(dir[0][2]);        dominant_axis = 2;    }             beam->dominant_axis = dominant_axis;    /*     * Precompute sign of direction.     * We know all 4 directions has same sign, thus dir[0] is used to     * get sign of direction for each axis.     */    beam->dirsign[0] = (dir[0][0] < 0.0) ? 1 : 0;    beam->dirsign[1] = (dir[0][1] < 0.0) ? 1 : 0;    beam->dirsign[2] = (dir[0][2] < 0.0) ? 1 : 0;    /*     * Project beam dir onto axis-alied plane.     */    {        ri_vector_t normals[3] = {            { 1.0, 0.0, 0.0 }, { 0.0, 1.0, 0.0 }, { 0.0, 0.0, 1.0 } };        ri_vector_t normal;        ri_float_t  t;        ri_float_t  k;        vcpy( normal, normals[beam->dominant_axis] );                if (beam->dirsign[beam->dominant_axis]) {            vneg( normal );        }        for (i = 0; i < 4; i++) {            t = vdot( dir[i], normal );                        if (fabs(t) > RI_EPS) {                k = beam->d / t;            } else {                k = 1.0;            }            beam->dir[i][0] = k * dir[i][0];            beam->dir[i][1] = k * dir[i][1];            beam->dir[i][2] = k * dir[i][2];                    }    }    /*     * Precompute inverse of direction     */    for (i = 0; i < 4; i++) {        beam->invdir[i][0] = safeinv( beam->dir[i][0], RI_EPS, RI_FLT_MAX );        beam->invdir[i][1] = safeinv( beam->dir[i][1], RI_EPS, RI_FLT_MAX );        beam->invdir[i][2] = safeinv( beam->dir[i][2], RI_EPS, RI_FLT_MAX );    }    /*     * Precompute normal plane of beam frustum     */    vcross( beam->normal[0], beam->dir[1], beam->dir[0] );    vcross( beam->normal[1], beam->dir[2], beam->dir[1] );    vcross( beam->normal[2], beam->dir[3], beam->dir[2] );    vcross( beam->normal[3], beam->dir[0], beam->dir[3] );    beam->is_tetrahedron = 0;    return 0;   /* OK   */}

 /** Set MadjT to transpose of inverse of M times determinant of M **/ void adjoint_transpose(_HMatrix M, _HMatrix MadjT) {     vcross(M[1], M[2], MadjT[0]);     vcross(M[2], M[0], MadjT[1]);     vcross(M[0], M[1], MadjT[2]); }

int main(int argc, char *argv[]){    (void)argc; (void)argv;#ifdef __SSE__    printf("SSE ");#endif#ifdef __SSE2__    printf("SSE2 ");#endif#ifdef __SSE3__    printf("SSE3 ");#endif#ifdef __SSE4__    printf("SSE4 ");#endif#ifdef __SSE4_1__    printf("SSE4.1 ");#endif#ifdef __SSE4_2__    printf("SSE4.2 ");#endif#ifdef __AVX__    printf("AVX ");#endif#ifdef __FMA4__    printf("FMA4 ");#endif    printf("/n");    printv(vec(1, 2, 3, 4));    printv(vzero());    printm(mzero());    printm(midentity());    vec4 a = { 1, 2, 3, 4 }, b = { 5, 6, 7, 8 };    printv(a);    printv(b);    printf("/nshuffles:/n");    printv(vshuffle(a, a, 0, 1, 2, 3));    printv(vshuffle(a, a, 3, 2, 1, 0));    printv(vshuffle(a, b, 0, 1, 0, 1));    printv(vshuffle(a, b, 2, 3, 2, 3));    printf("/ndot products:/n");    printv(vdot(a, b));    printv(vdot(b, a));    printv(vdot3(a, b));    printv(vdot3(b, a));    //vec4 blendmask = { 1, -1, 1, -1 };    //printv(vblend(x, y, blendmask));    vec4 x = { 1, 0, 0, 0 }, y = { 0, 1, 0, 0 }, z = { 0, 0, 1, 0 }, w = { 0, 0, 0, 1 };    printf("/ncross products:/n");    printv(vcross(x, y));    printv(vcross(y, x));    printv(vcross_scalar(x, y));    printv(vcross_scalar(y, x));    printf("/nquaternion products:/n");    printv(qprod(x, y));    printv(qprod(y, x));    printv(qprod_mad(x, y));    printv(qprod_mad(y, x));    printv(qprod_scalar(x, y));    printv(qprod_scalar(y, x));    printf("/nquaternion conjugates:/n");    printv(qconj(x));    printv(qconj(y));    printv(qconj(z));    printv(qconj(w));    printf("/nmat from quat:/n");    printm(quat_to_mat(w));    printm(quat_to_mat_mmul(w));    printm(quat_to_mat_scalar(w));    vec4 angles = { 0.0, 0.0, 0.0, 0.0 };    printf("/neuler to quaternion:/n");    printv(quat_euler(angles));    printv(quat_euler_scalar(angles));    printv(quat_euler_gems(angles));    printf("/neuler to matrix:/n");    printm(mat_euler(angles));    printm(mat_euler_scalar(angles));    printm(quat_to_mat(quat_euler(angles)));//.........这里部分代码省略.........