//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.
*/
int
find_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);
}
}
void
add_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 load
vec3 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 void
trackball ( 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 void
orthoBasis(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 void
orthoBasis(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];
}
void
add_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 0
printf("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 0
printf("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 );
}
void PhysArbiter::ApplyImpulse()
{
Body* b1 = body1;
Body* b2 = body2;
for(int i=0;i<numContacts;++i)
{
PhysContact *c = &contacts[i];
float2 r1 = c->pos - b1->pos;
float2 r2 = c->pos - b2->pos;
float2 dv = b2->velocity +
vcross(b2->angularVelocity, r2) -
b1->velocity - vcross(b1->angularVelocity, r1);
float vn = dv.dot(c->normal);
float dPn = c->massNormal * (-vn + c->bias);
/*float Pn0 = c->accNormalImpulse;
c->accNormalImpulse = max(Pn0 + dPn, 0.0f);
dPn = c->accNormalImpulse - Pn0;*/
dPn = max(dPn,0);
float2 Pn = c->normal*dPn;
b1->velocity -= Pn*b1->invMass;
b1->angularVelocity -= b1->invInertia * vcross(r1, Pn);
b2->velocity += Pn * b2->invMass;
b2->angularVelocity += b2->invInertia * vcross(r2, Pn);
}
/*for (int i = 0; i < numContacts; ++i)
{
PhysContact* c = contacts + i;
float2 r1 = c->pos - b1->pos;
float2 r2 = c->pos - b2->pos;
// relative velocity at contact
float2 relativeVelocity = b2->velocity + vcross(b2->angularVelocity, r2) -
b1->velocity - vcross(b1->angularVelocity, r1);
// normal impulse
float vn = relativeVelocity.dot(c->normal);
float normalImpulse = c->massNormal * (-vn + c->bias);
//float normalImpulse = c->massNormal * (c->restitution - vn);
// clamp accumulated impulse
float oldNormalImpulse = c->accNormalImpulse;
c->accNormalImpulse = max(oldNormalImpulse + normalImpulse, 0.0f);
normalImpulse = c->accNormalImpulse - oldNormalImpulse;
// apply contact impulse
float2 impulse = c->normal*normalImpulse;
b1->velocity -= impulse*b1->invMass;
b1->angularVelocity -= b1->invInertia * vcross(r1, impulse);
b2->velocity += impulse*b2->invMass;
b2->angularVelocity += b2->invInertia * vcross(r2, impulse);
//relativeVelocity.set(b2.getBiasedVelocity());
//relativeVelocity.add(MathUtil.cross(b2.getBiasedAngularVelocity(), r2));
//relativeVelocity.sub(b1.getBiasedVelocity());
//relativeVelocity.sub(MathUtil.cross(b1.getBiasedAngularVelocity(), r1));
//float vnb = relativeVelocity.dot(c.normal);
//float biasImpulse = c.massNormal * (-vnb + c.bias);
//float oldBiasImpulse = c.biasImpulse;
//c.biasImpulse = Math.max(oldBiasImpulse + biasImpulse, 0.0f);
//biasImpulse = c.biasImpulse - oldBiasImpulse;
//Vector2f Pb = MathUtil.scale(c.normal, biasImpulse);
//
//b1.adjustBiasedVelocity(MathUtil.scale(Pb, -b1.getInvMass()));
//b1.adjustBiasedAngularVelocity(-(b1.getInvI() * MathUtil.cross(r1, Pb)));
//b2.adjustBiasedVelocity(MathUtil.scale(Pb, b2.getInvMass()));
//b2.adjustBiasedAngularVelocity((b2.getInvI() * MathUtil.cross(r2, Pb)));
float maxTangentImpulse = friction * c->accNormalImpulse;
relativeVelocity = b2->velocity + vcross(b2->angularVelocity,r2)
- b1->velocity - vcross(b1->angularVelocity, r1);
float2 tangent = vcross(c->normal, 1.0f);
float vt = relativeVelocity.dot(tangent);
float tangentImpulse = c->massTangent * (-vt);
float oldTangentImpulse = c->accTangentImpulse;
c->accTangentImpulse = clamp(oldTangentImpulse+tangentImpulse,
-maxTangentImpulse, maxTangentImpulse);
tangentImpulse = c->accTangentImpulse - oldTangentImpulse;
impulse = tangent*tangentImpulse;
b1->velocity -= impulse*b1->invMass;
b1->angularVelocity -= b1->invInertia * vcross(r1,impulse);
b2->velocity += impulse*b2->invMass;
b2->angularVelocity += b2->invInertia * vcross(r2, impulse);
}*/
};
/*
* Function: ri_bem_set
*
* Setups a beam structure.
*
*
* Parameters:
*
* beam - Pointer to the beam to be set up.
* org - Origin of beam.
* dir[4] - Corner rays of beam.
*
*
* Returns:
*
* 0 if OK, otherwise if err
*/
int
ri_beam_set(
ri_beam_t *beam, /* [inout] */
ri_vector_t org,
ri_vector_t dir[4])
{
int i, j;
int mask;
int dominant_axis;
int zeros;
ri_float_t maxval;
beam->d = 1024.0;
beam->t_max = RI_INFINITY;
/*
* Check if beam's directions lie in same quadrant.
*/
for (i = 0; i < 3; i++) {
zeros = 0;
mask = 0;
for (j = 0; j < 4; j++) {
if (fabs(dir[j][i]) < RI_EPS) {
zeros++;
} else {
mask += (dir[j][i] < 0.0) ? 1 : -1;
}
}
if ( (mask != -(4 - zeros)) && (mask != (4 - zeros)) ) {
/* FIXME:
* split beam so that subdivided beam has same sign.
*/
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)));
printf("\nperspective matrix:\n");
printm(mat_perspective_fovy(M_PI/4.0, 16.0/9.0, 0.1, 100.0));
printm(mat_perspective_fovy_inf_z(M_PI/4.0, 16.0/9.0, 0.1));
printm(mat_perspective_fovy_scalar(M_PI/4.0, 16.0/9.0, 0.1, 100.0));
printf("\northogonal matrix:\n");
printm(mat_ortho(-1.0, 1.0, -1.0, 1.0, -1.0, 1.0));
printm(mat_ortho(-1.0, 2.0, -1.0, 2.0, -1.0, 2.0));
printf("\ntranslate matrix:\n");
printm(mtranslate(a));
printf("\nscale matrix:\n");
printm(mscale(a));
return 0;
}
