int lines_intersect(int i,int j,int k,int l,ls lss[MAX_LS_AMOUNT])
/* checks if two lines intersect on 3D sphere
see theory in paper Pulkki, V. Lokki, T. "Creating Auditory Displays
with Multiple Loudspeakers Using VBAP: A Case Study with
DIVA Project" in International Conference on
Auditory Displays -98. E-mail [email protected]
if you want to have that paper.*/
{
cart_vec v1;
cart_vec v2;
cart_vec v3, neg_v3;
float angle;
float dist_ij,dist_kl,dist_iv3,dist_jv3,dist_inv3,dist_jnv3;
float dist_kv3,dist_lv3,dist_knv3,dist_lnv3;
cross_prod(lss[i].coords,lss[j].coords,&v1);
cross_prod(lss[k].coords,lss[l].coords,&v2);
cross_prod(v1,v2,&v3);
neg_v3.x= 0.0 - v3.x;
neg_v3.y= 0.0 - v3.y;
neg_v3.z= 0.0 - v3.z;
dist_ij = (vec_angle(lss[i].coords,lss[j].coords));
dist_kl = (vec_angle(lss[k].coords,lss[l].coords));
dist_iv3 = (vec_angle(lss[i].coords,v3));
dist_jv3 = (vec_angle(v3,lss[j].coords));
dist_inv3 = (vec_angle(lss[i].coords,neg_v3));
dist_jnv3 = (vec_angle(neg_v3,lss[j].coords));
dist_kv3 = (vec_angle(lss[k].coords,v3));
dist_lv3 = (vec_angle(v3,lss[l].coords));
dist_knv3 = (vec_angle(lss[k].coords,neg_v3));
dist_lnv3 = (vec_angle(neg_v3,lss[l].coords));
/* if one of loudspeakers is close to crossing point, don't do anything*/
if(fabsf(dist_iv3) <= 0.01 || fabsf(dist_jv3) <= 0.01 ||
fabsf(dist_kv3) <= 0.01 || fabsf(dist_lv3) <= 0.01 ||
fabsf(dist_inv3) <= 0.01 || fabsf(dist_jnv3) <= 0.01 ||
fabsf(dist_knv3) <= 0.01 || fabsf(dist_lnv3) <= 0.01 )
return(0);
if (((fabsf(dist_ij - (dist_iv3 + dist_jv3)) <= 0.01 ) &&
(fabsf(dist_kl - (dist_kv3 + dist_lv3)) <= 0.01)) ||
((fabsf(dist_ij - (dist_inv3 + dist_jnv3)) <= 0.01) &&
(fabsf(dist_kl - (dist_knv3 + dist_lnv3)) <= 0.01 ))) {
return (1);
} else {
return (0);
}
}
static int lines_intersect(int i,int j,int k,int l,t_ls lss[MAX_LS_AMOUNT])
/* checks if two lines intersect on 3D sphere
*/
{
t_ls v1;
t_ls v2;
t_ls v3, neg_v3;
//t_float angle;
t_float dist_ij,dist_kl,dist_iv3,dist_jv3,dist_inv3,dist_jnv3;
t_float dist_kv3,dist_lv3,dist_knv3,dist_lnv3;
// TODO epsilon needs to be updated for 64-bit/double precision
t_float epsilon = 1e-9;
ls_cross_prod(lss[i],lss[j],&v1);
ls_cross_prod(lss[k],lss[l],&v2);
ls_cross_prod(v1,v2,&v3);
neg_v3.x= 0.0 - v3.x;
neg_v3.y= 0.0 - v3.y;
neg_v3.z= 0.0 - v3.z;
dist_ij = (vec_angle(lss[i],lss[j]));
dist_kl = (vec_angle(lss[k],lss[l]));
dist_iv3 = (vec_angle(lss[i],v3));
dist_jv3 = (vec_angle(v3,lss[j]));
dist_inv3 = (vec_angle(lss[i],neg_v3));
dist_jnv3 = (vec_angle(neg_v3,lss[j]));
dist_kv3 = (vec_angle(lss[k],v3));
dist_lv3 = (vec_angle(v3,lss[l]));
dist_knv3 = (vec_angle(lss[k],neg_v3));
dist_lnv3 = (vec_angle(neg_v3,lss[l]));
/* if one of loudspeakers is close to crossing point, don't do anything*/
if(fabsf(dist_iv3) <= epsilon || fabsf(dist_jv3) <= epsilon ||
fabsf(dist_kv3) <= epsilon || fabsf(dist_lv3) <= epsilon ||
fabsf(dist_inv3) <= epsilon || fabsf(dist_jnv3) <= epsilon ||
fabsf(dist_knv3) <= epsilon || fabsf(dist_lnv3) <= epsilon )
return(0);
// if crossing point is on line between both loudspeakers return 1
if (((fabsf(dist_ij - (dist_iv3 + dist_jv3)) <= epsilon) &&
(fabsf(dist_kl - (dist_kv3 + dist_lv3)) <= epsilon)) ||
((fabsf(dist_ij - (dist_inv3 + dist_jnv3)) <= epsilon) &&
(fabsf(dist_kl - (dist_knv3 + dist_lnv3)) <= epsilon))) {
return (1);
} else {
return (0);
}
}
int lines_intersect(int i,int j,int k,int l,t_ls lss[MAX_LS_AMOUNT])
/* checks if two lines intersect on 3D sphere
*/
{
t_ls v1;
t_ls v2;
t_ls v3, neg_v3;
//float angle;
float dist_ij,dist_kl,dist_iv3,dist_jv3,dist_inv3,dist_jnv3;
float dist_kv3,dist_lv3,dist_knv3,dist_lnv3;
ls_cross_prod(lss[i],lss[j],&v1);
ls_cross_prod(lss[k],lss[l],&v2);
ls_cross_prod(v1,v2,&v3);
neg_v3.x= 0.0 - v3.x;
neg_v3.y= 0.0 - v3.y;
neg_v3.z= 0.0 - v3.z;
dist_ij = (vec_angle(lss[i],lss[j]));
dist_kl = (vec_angle(lss[k],lss[l]));
dist_iv3 = (vec_angle(lss[i],v3));
dist_jv3 = (vec_angle(v3,lss[j]));
dist_inv3 = (vec_angle(lss[i],neg_v3));
dist_jnv3 = (vec_angle(neg_v3,lss[j]));
dist_kv3 = (vec_angle(lss[k],v3));
dist_lv3 = (vec_angle(v3,lss[l]));
dist_knv3 = (vec_angle(lss[k],neg_v3));
dist_lnv3 = (vec_angle(neg_v3,lss[l]));
/* if one of loudspeakers is close to crossing point, don't do anything*/
if(fabsf(dist_iv3) <= 0.01 || fabsf(dist_jv3) <= 0.01 ||
fabsf(dist_kv3) <= 0.01 || fabsf(dist_lv3) <= 0.01 ||
fabsf(dist_inv3) <= 0.01 || fabsf(dist_jnv3) <= 0.01 ||
fabsf(dist_knv3) <= 0.01 || fabsf(dist_lnv3) <= 0.01 )
return(0);
// if crossing point is on line between both loudspeakers return 1
if (((fabsf(dist_ij - (dist_iv3 + dist_jv3)) <= 0.01 ) &&
(fabsf(dist_kl - (dist_kv3 + dist_lv3)) <= 0.01)) ||
((fabsf(dist_ij - (dist_inv3 + dist_jnv3)) <= 0.01) &&
(fabsf(dist_kl - (dist_knv3 + dist_lnv3)) <= 0.01 ))) {
return (1);
} else {
return (0);
}
}
static void ship_draw(particle_t *p){
float angle = vec_angle(p->box.axis0);
/*particle_draw_square(p);*/
model_draw_shadow(p->model[0],p->box.pos.x-5,p->box.pos.y-5,-50,16.0,angle - 90);
model_draw(p->model[0],p->box.pos.x,p->box.pos.y,0,15.0,angle - 90);
}
float vol_p_side_lgth(int i, int j,int k, ls lss[MAX_LS_AMOUNT] ){
/* calculate volume of the parallelepiped defined by the loudspeaker
direction vectors and divide it with total length of the triangle sides.
This is used when removing too narrow triangles. */
float volper, lgth;
cart_vec xprod;
cross_prod(lss[i].coords, lss[j].coords, &xprod);
volper = fabsf(vec_prod(xprod, lss[k].coords));
lgth = (fabsf(vec_angle(lss[i].coords,lss[j].coords))
+ fabsf(vec_angle(lss[i].coords,lss[k].coords))
+ fabsf(vec_angle(lss[j].coords,lss[k].coords)));
if(lgth>0.00001)
return volper / lgth;
else
return 0.0;
}
void quat_from_vec_pair(const Vec3f a, const Vec3f b, Quat q) {
Vec4f axis;
vec_cross(a,b,axis);
float angle;
vec_angle(a,b,&angle);
if( (fabs(axis[0]) < CUTE_EPSILON && fabs(axis[1]) < CUTE_EPSILON && fabs(axis[2]) < CUTE_EPSILON) ||
fabs(angle) < CUTE_EPSILON )
{
quat_identity(q);
}
quat_from_axis_angle(axis, angle, q);
}
int configure(int a){
int i;
if(a){
L = 20;
d = 1;
offset = 0.25;
det = 55;
mask = 85;
height = 20;
thick = 0.2;
alpha = 0.1*PI;
beta = 0.3*PI;
sp = n();
opening= op();
PI2_over_256 = 2*PI/256;
PIL_over_d = PI*L/d;
st_ob = sp*opening;
det_2 = det;
det_2_sq = pow(det_2,2);
det *= 0.5;
mask *= 0.5;
sp_2 = sp*sp;
}
for(i=0;i<6;i++) offset_PI[i] = (offset+i)*PI;
double tel_axis[3], rot_axis[3], sun_axis[3], x_axis[3];
tel_axis[0] = sin(beta)*cos(0); tel_axis[1] = sin(beta)*sin(0); tel_axis[2] = cos(beta);
rot_axis[0] = 0.; rot_axis[1] = 0.; rot_axis[2] = 1.;
x_axis[0] = 1.; x_axis[1] = 0; x_axis[2] = 0;
vec_ort_wc(sun_axis,rot_axis,tel_axis);
vec_scap(-1,sun_axis);
vec_rotate(tel_axis,-alpha,sun_axis);
alpha_proj = vec_angle(rot_axis,sun_axis,x_axis);
return 0;
}
box_t box_direct(box_t b, vec_t direction){
return box_rotate(b, vec_angle(direction) - vec_angle(b.axis0) );
}
static void missile_draw(particle_t *p){
float angle = vec_angle(p->box.axis0);
model_draw_shadow(p->model[0],p->box.pos.x-5,p->box.pos.y-5,-51,8.0,angle - 90);
model_draw(p->model[0],p->box.pos.x,p->box.pos.y,-10,7.0,angle - 90);
}
void choose_ls_triplets(ls lss[MAX_LS_AMOUNT],
struct ls_triplet_chain **ls_triplets, int ls_amount)
/* Selects the loudspeaker triplets, and
calculates the inversion matrices for each selected triplet.
A line (connection) is drawn between each loudspeaker. The lines
denote the sides of the triangles. The triangles should not be
intersecting. All crossing connections are searched and the
longer connection is erased. This yields non-intesecting triangles,
which can be used in panning.*/
{
int i,j,k,l,m,li, table_size;
int *i_ptr;
cart_vec vb1,vb2,tmp_vec;
int connections[MAX_LS_AMOUNT][MAX_LS_AMOUNT];
float angles[MAX_LS_AMOUNT];
int sorted_angles[MAX_LS_AMOUNT];
float distance_table[((MAX_LS_AMOUNT * (MAX_LS_AMOUNT - 1)) / 2)];
int distance_table_i[((MAX_LS_AMOUNT * (MAX_LS_AMOUNT - 1)) / 2)];
int distance_table_j[((MAX_LS_AMOUNT * (MAX_LS_AMOUNT - 1)) / 2)];
float distance;
struct ls_triplet_chain *trip_ptr, *prev, *tmp_ptr;
if (ls_amount == 0) {
fprintf(stderr,"Number of loudspeakers is zero\nExiting\n");
exit(-1);
}
for(i=0;i<ls_amount;i++)
for(j=i+1;j<ls_amount;j++)
for(k=j+1;k<ls_amount;k++){
if(vol_p_side_lgth(i,j, k, lss) > MIN_VOL_P_SIDE_LGTH){
connections[i][j]=1;
connections[j][i]=1;
connections[i][k]=1;
connections[k][i]=1;
connections[j][k]=1;
connections[k][j]=1;
add_ldsp_triplet(i,j,k,ls_triplets, lss);
}
}
/*calculate distancies between all lss and sorting them*/
table_size =(((ls_amount - 1) * (ls_amount)) / 2);
for(i=0;i<table_size; i++)
distance_table[i] = 100000.0;
for(i=0;i<ls_amount;i++){
for(j=(i+1);j<ls_amount; j++){
if(connections[i][j] == 1) {
distance = fabs(vec_angle(lss[i].coords,lss[j].coords));
k=0;
while(distance_table[k] < distance)
k++;
for(l=(table_size - 1);l > k ;l--){
distance_table[l] = distance_table[l-1];
distance_table_i[l] = distance_table_i[l-1];
distance_table_j[l] = distance_table_j[l-1];
}
distance_table[k] = distance;
distance_table_i[k] = i;
distance_table_j[k] = j;
} else
table_size--;
}
}
/* disconnecting connections which are crossing shorter ones,
starting from shortest one and removing all that cross it,
and proceeding to next shortest */
for(i=0; i<(table_size); i++){
int fst_ls = distance_table_i[i];
int sec_ls = distance_table_j[i];
if(connections[fst_ls][sec_ls] == 1)
for(j=0; j<ls_amount ; j++)
for(k=j+1; k<ls_amount; k++)
if( (j!=fst_ls) && (k != sec_ls) && (k!=fst_ls) && (j != sec_ls)){
if(lines_intersect(fst_ls, sec_ls, j,k,lss) == 1){
connections[j][k] = 0;
connections[k][j] = 0;
}
}
}
/* remove triangles which had crossing sides
with smaller triangles or include loudspeakers*/
trip_ptr = *ls_triplets;
prev = NULL;
while (trip_ptr != NULL){
i = trip_ptr->ls_nos[0];
j = trip_ptr->ls_nos[1];
k = trip_ptr->ls_nos[2];
if(connections[i][j] == 0 ||
connections[i][k] == 0 ||
connections[j][k] == 0 ||
any_ls_inside_triplet(i,j,k,lss,ls_amount) == 1 ){
if(prev != NULL) {
prev->next = trip_ptr->next;
tmp_ptr = trip_ptr;
trip_ptr = trip_ptr->next;
free(tmp_ptr);
} else {
*ls_triplets = trip_ptr->next;
tmp_ptr = trip_ptr;
trip_ptr = trip_ptr->next;
free(tmp_ptr);
}
} else {
prev = trip_ptr;
trip_ptr = trip_ptr->next;
}
}
}
static void choose_ls_triplets(t_def_ls *x)
/* Selects the loudspeaker triplets, and
calculates the inversion matrices for each selected triplet.
A line (connection) is drawn between each loudspeaker. The lines
denote the sides of the triangles. The triangles should not be
intersecting. All crossing connections are searched and the
longer connection is erased. This yields non-intesecting triangles,
which can be used in panning.
See theory in paper Pulkki, V. Lokki, T. "Creating Auditory Displays
with Multiple Loudspeakers Using VBAP: A Case Study with
DIVA Project" in International Conference on
Auditory Displays -98.*/
{
int i,j,k,l,/*m,li,*/ table_size;
//int *i_ptr;
//t_ls vb1,vb2,tmp_vec;
int connections[MAX_LS_AMOUNT][MAX_LS_AMOUNT];
//float angles[MAX_LS_AMOUNT];
//int sorted_angles[MAX_LS_AMOUNT];
t_float distance_table[((MAX_LS_AMOUNT * (MAX_LS_AMOUNT - 1)) / 2)];
int distance_table_i[((MAX_LS_AMOUNT * (MAX_LS_AMOUNT - 1)) / 2)];
int distance_table_j[((MAX_LS_AMOUNT * (MAX_LS_AMOUNT - 1)) / 2)];
t_float distance;
t_ls_set *trip_ptr, *prev, *tmp_ptr;
int ls_amount = x->x_def_ls_amount;
t_ls *lss = x->x_ls;
if (ls_amount == 0) { post("define-loudspeakers: Number of loudspeakers is zero"); return; }
for(i=0;i<ls_amount;i++)
for(j=i+1;j<ls_amount;j++)
for(k=j+1;k<ls_amount;k++)
{
if(vol_p_side_lgth(i,j,k, x->x_ls) > MIN_VOL_P_SIDE_LGTH)
{
connections[i][j]=1;
connections[j][i]=1;
connections[i][k]=1;
connections[k][i]=1;
connections[j][k]=1;
connections[k][j]=1;
add_ldsp_triplet(i,j,k,x);
}
}
/*calculate distancies between all lss and sorting them*/
table_size =(((ls_amount - 1) * (ls_amount)) / 2);
for(i=0;i<table_size; i++)
distance_table[i] = 100000.0;
for(i=0;i<ls_amount;i++)
{
for(j=(i+1);j<ls_amount; j++)
{
if(connections[i][j] == 1)
{
distance = fabs(vec_angle(lss[i],lss[j]));
k=0;
while(distance_table[k] < distance)
k++;
for(l=(table_size - 1);l > k ;l--)
{
distance_table[l] = distance_table[l-1];
distance_table_i[l] = distance_table_i[l-1];
distance_table_j[l] = distance_table_j[l-1];
}
distance_table[k] = distance;
distance_table_i[k] = i;
distance_table_j[k] = j;
}
else
{
table_size--;
}
}
}
/* disconnecting connections which are crossing shorter ones,
starting from shortest one and removing all that cross it,
and proceeding to next shortest */
for(i=0; i<(table_size); i++)
{
int fst_ls = distance_table_i[i];
int sec_ls = distance_table_j[i];
if(connections[fst_ls][sec_ls] == 1)
{
for(j=0; j<ls_amount ; j++)
{
for(k=j+1; k<ls_amount; k++)
{
if( (j!=fst_ls) && (k != sec_ls) && (k!=fst_ls) && (j != sec_ls))
{
if(lines_intersect(fst_ls, sec_ls, j,k,x->x_ls) == 1)
{
connections[j][k] = 0;
connections[k][j] = 0;
}
}
}
}
}
}
/* remove triangles which had crossing sides
with smaller triangles or include loudspeakers*/
trip_ptr = x->x_ls_set;
prev = NULL;
while (trip_ptr != NULL)
{
i = trip_ptr->ls_nos[0];
j = trip_ptr->ls_nos[1];
k = trip_ptr->ls_nos[2];
if(connections[i][j] == 0 ||
connections[i][k] == 0 ||
connections[j][k] == 0 ||
any_ls_inside_triplet(i,j,k,x->x_ls,ls_amount) == 1 )
{
if(prev != NULL)
{
prev->next = trip_ptr->next;
tmp_ptr = trip_ptr;
trip_ptr = trip_ptr->next;
freebytes(tmp_ptr, sizeof (struct t_ls_set));
}
else
{
x->x_ls_set = trip_ptr->next;
tmp_ptr = trip_ptr;
trip_ptr = trip_ptr->next;
freebytes(tmp_ptr, sizeof (struct t_ls_set));
}
}
else
{
prev = trip_ptr;
trip_ptr = trip_ptr->next;
}
}
x->x_triplets_specified=1;
}
void surf3::computeCurvature( const levelset3 *solid ) {
// Build vertex to vertex info
std::vector<std::vector<uint> > v2v(vertices.size());
for( uint n=0; n<faces.size(); n++ ) {
for( uint m1=0; m1<DIM; m1++ ) for( uint m2=0; m2<DIM; m2++ ) {
if( m1 != m2 ) {
bool duplicated = false;
for( uint i=0; i<v2v[faces[n][m1]].size(); i++ ) {
if( v2v[faces[n][m1]][i] == faces[n][m2] ) {
duplicated = true;
break;
}
}
if( ! duplicated ) v2v[faces[n][m1]].push_back(faces[n][m2]);
}
}
}
// Build vertex to face info
std::vector<std::vector<uint> > v2faces(vertices.size());
for( uint n=0; n<faces.size(); n++ ) {
for( uint m=0; m<DIM; m++ ) {
v2faces[faces[n][m]].push_back(n);
}
}
// Build segments and vertex to segment info
std::vector<std::vector<uint> > segments;
std::vector<std::vector<uint> > v2segs(vertices.size());
for( uint n=0; n<v2v.size(); n++ ) {
for( uint m=0; m<v2v[n].size(); m++ ) {
if( n < v2v[n][m] ) {
std::vector<uint> seg(2);
seg[0] = n;
seg[1] = v2v[n][m];
segments.push_back(seg);
v2segs[seg[0]].push_back(segments.size()-1);
v2segs[seg[1]].push_back(segments.size()-1);
}
}
}
// Build seg2face matrix
std::vector<std::vector<uint> > seg2faces(segments.size());
for( uint n=0; n<segments.size(); n++ ) {
uint idx0 = segments[n][0];
uint idx1 = segments[n][1];
for( uint m=0; m<v2faces[idx0].size(); m++ ) {
for( int k=DIM-1; k>=0; k-- ) {
if( faces[v2faces[idx0][m]][k] == idx1 ) {
seg2faces[n].push_back(v2faces[idx0][m]);
break;
}
}
}
}
// Now begin computing the curvature
curvature.resize(vertices.size());
PARALLEL_FOR for( uint n=0; n<vertices.size(); n++ ) {
vec3d kn;
for( uint m=0; m<v2segs[n].size(); m++ ) {
vec3d x0, x1;
uint seg = v2segs[n][m];
x0 = vertices[n];
for( uint i=0; i<2; i++ ) if( segments[seg][i] != n ) {
x1 = vertices[segments[seg][i]];
}
for( uint f=0; f<seg2faces[seg].size(); f++ ) {
uint face = seg2faces[seg][f];
// Find opposite vertex
int opv = -1;
for( uint i=0; i<3; i++ ) {
if( faces[face][i] != segments[seg][0] && faces[face][i] != segments[seg][1] ) {
opv = i;
break;
}
}
if( opv >= 0 ) {
vec3d v1 = vertices[faces[face][(opv+1)%3]]-vertices[faces[face][opv]];
vec3d v2 = vertices[faces[face][(opv+2)%3]]-vertices[faces[face][opv]];
FLOAT64 angle = vec_angle(v1,v2);
FLOAT64 k = cos(angle)/(1e-16+sin(angle));
kn += k*(x1-x0);
}
}
}
curvature[n] = kn;
}
ringArea.resize(vertices.size());
PARALLEL_FOR for( uint n=0; n<vertices.size(); n++ ) {
FLOAT64 area_sum = 0.0;
for( uint m=0; m<v2faces[n].size(); m++ ) {
uint face = v2faces[n][m];
vec3d v1 = vertices[faces[face][1]]-vertices[faces[face][0]];
vec3d v2 = vertices[faces[face][2]]-vertices[faces[face][0]];
FLOAT64 area = 0.5 * (v1 ^ v2).len();
if( ! area ) vertices[faces[face][0]] += 1e-8*vec3d(nrand(),nrand(),nrand());
area_sum += area;
}
area_sum = fmax(1e-18,area_sum);
ringArea[n] = area_sum/3.0;
curvature[n] = curvature[n] / ringArea[n];
}
// Set curvature zero inside the solid
if( solid ) {
for( uint n=0; n<vertices.size(); n++ ) {
if( solid->evalLevelset(vertices[n]) < 0.0 ) {
curvature[n] = vec3d();
ringArea[n] = 0.0;
}
}
}
#if 1
// Smoothing part
for( uint k=0; k<2; k++ ) {
std::vector<vec3d> old_curvature = curvature;
PARALLEL_FOR for( uint n=0; n<vertices.size(); n++ ) {
vec3d new_curvature;
FLOAT64 wsum = 0.0;
for( uint m=0; m<v2v[n].size(); m++ ) {
FLOAT64 w = ringArea[v2v[n][m]];
wsum += w;
new_curvature += w*old_curvature[v2v[n][m]];
}
FLOAT64 w = ringArea[n];
new_curvature += w*old_curvature[n];
wsum += w;
if( wsum ) {
curvature[n] = new_curvature / wsum;
}
}
}
#endif
}
int32_t pivot_lookat(struct Pivot* pivot, const Vec3f target) {
int32_t result = -1;
Vec4f right_axis = RIGHT_AXIS;
Vec4f up_axis = UP_AXIS;
Vec4f forward_axis = FORWARD_AXIS;
struct Pivot world_pivot;
pivot_combine(pivot->parent, pivot, &world_pivot);
Vec4f target_direction;
vec_sub(target, world_pivot.position, target_direction);
float dot_yaw = vec_dot(target_direction, forward_axis);
Quat rotation = {0};
if( fabs(dot_yaw + 1.0f) < CUTE_EPSILON ) {
// vector a and b point exactly in the opposite direction,
// so it is a 180 degrees turn around the up-axis
Quat rotation = {0};
quat_from_axis_angle(up_axis, PI, rotation);
quat_mul(world_pivot.orientation, rotation, rotation);
} else if( fabs(dot_yaw - (1.0f)) < CUTE_EPSILON ) {
// vector a and b point exactly in the same direction
// so we return the identity quaternion
quat_copy(world_pivot.orientation, rotation);
} else {
// - I look at the target by turning the pivot orientation using only
// yaw and pitch movement
// - I always rotate the pivot from its initial orientation (up and forward
// basis vectors like initialized above), so this does not incrementally
// advance the orientation
quat_identity(rotation);
// - to find the amount of yaw I project the target_direction into the
// up_axis plane, resulting in up_projection which is a vector that
// points from the up_axis plane to the tip of the target_direction
Vec4f up_projection = {0};
vec_mul1f(up_axis, vec_dot(target_direction, up_axis), up_projection);
// - so then by subtracting the up_projection from the target_direction,
// I get a vector lying in the up_axis plane, pointing towards the target
Vec4f yaw_direction = {0};
vec_sub(target_direction, up_projection, yaw_direction);
// - angle between yaw_direction and forward_axis is the amount of yaw we
// need to point the forward_axis toward the target
float yaw = 0.0f;
vec_angle(yaw_direction, forward_axis, &yaw);
log_assert( ! isnan(yaw),
"vec_angle(%f %f %f, %f %f %f, %f);\n",
yaw_direction[0], yaw_direction[1], yaw_direction[2],
forward_axis[0], forward_axis[1], forward_axis[2],
yaw );
// - I have to compute the cross product between yaw_direction and
// forward_axis and use the resulting yaw_axis
Vec4f yaw_axis = {0};
vec_cross(yaw_direction, forward_axis, yaw_axis);
if( vec_nullp(yaw_axis) ) {
vec_copy4f(up_axis, yaw_axis);
}
// - compute the yaw rotation
Quat yaw_rotation = {0};
quat_from_axis_angle(yaw_axis, yaw, yaw_rotation);
// - to compute, just as with the yaw, I want an axis that lies on the plane that
// is spanned in this case by the right_axis, when the camera points toward the
// target
// - I could compute an axis, but I already have a direction vector that points
// toward the target, the yaw_direction, I just have to normalize it to make it
// an axis (and put the result in forward_axis, since it now is the forward_axis
// of the yaw turned camera)
Vec4f yaw_forward_axis = {0};
vec_normalize(yaw_direction, yaw_forward_axis);
// - then use the new forward axis with the old target_direction to compute the angle
// between those
float pitch = 0.0f;
vec_angle(target_direction, yaw_forward_axis, &pitch);
log_assert( ! isnan(pitch),
"vec_angle(%f %f %f, %f %f %f, %f);\n",
target_direction[0], target_direction[1], target_direction[2],
yaw_forward_axis[0], yaw_forward_axis[1], yaw_forward_axis[2],
pitch );
// - and just as in the yaw case we compute an rotation pitch_axis
Vec4f pitch_axis = {0};
vec_cross(target_direction, yaw_forward_axis, pitch_axis);
if( vec_nullp(pitch_axis) ) {
vec_copy4f(right_axis, pitch_axis);
}
// - and finally compute the pitch rotation and combine it with the yaw_rotation
// in the same step
Quat pitch_rotation;
quat_from_axis_angle(pitch_axis, pitch, pitch_rotation);
Quat yaw_pitch_rotation;
quat_mul(yaw_rotation, pitch_rotation, yaw_pitch_rotation);
Quat inverted_orientation = {0};
quat_invert(world_pivot.orientation, inverted_orientation);
// - the int32_t I want to return indicates the cameras 'flip' status, that is, it is
// one when the camera angle was pitched so much that it flipped over and its
// up axis is now pointing downwards
// - to find out if I am flipped over, I compute the flipped up_axis called
// flip_axis and then use the dot product between the flip_axis and up_axis
// to decide if I am flipped
Vec4f flip_axis = {0};
vec_rotate(up_axis, inverted_orientation, flip_axis);
vec_rotate(flip_axis, yaw_pitch_rotation, flip_axis);
float dot_pitch = vec_dot(up_axis, flip_axis);
Vec4f target_axis = {0};
vec_normalize(target_direction, target_axis);
// - check if we are flipped and if we are, set result to 1 meaning we are flipped
// - turn the camera around PI so that we can continue pitching, otherwise we just get
// stuck when trying to flip the camera over
if( dot_pitch < 0.0f ) {
result = 1;
quat_from_axis_angle(target_axis, PI, rotation);
quat_mul(yaw_pitch_rotation, rotation, yaw_pitch_rotation);
}
quat_copy(yaw_pitch_rotation, rotation);
}
if( ! isnan(rotation[0]) &&
! isnan(rotation[1]) &&
! isnan(rotation[2]) &&
! isnan(rotation[3]) )
{
quat_copy(rotation, pivot->orientation);
}
pivot->eye_distance = vec_length(target_direction);
return result;
}
