void moto_object_node_zoom(MotoObjectNode *self, gfloat val)
{
gfloat to_target[3], eye[3];
gfloat loc_pos[] = {0, 0, 0};
gfloat *matrix = moto_object_node_get_matrix(self, TRUE);
point3_transform(eye, matrix, loc_pos);
vector3_dif(to_target, self->priv->target, eye);
if(vector3_length(to_target) < MICRO)
{
if(val > 0)
return;
gfloat az[] = {0, 0, 1};
gfloat n[3];
vector3_transform(n, matrix, az);
point3_move(eye, n, 0.5);
}
else
point3_move(eye, to_target, val);
if(self->priv->parent)
{
gfloat *inverse = moto_object_node_get_inverse_matrix(self->priv->parent, TRUE);
point3_transform(loc_pos, inverse, eye);
}
else
{
vector3_copy(loc_pos, eye);
}
moto_object_node_set_translate_array(self, loc_pos);
}
int moto_ray_intersect_cylinder_dist(MotoRay *self,
float *dist, float a[3], float b[3], float radius)
{
const float z[3] = {0, 0, 1};
float c[3], tmp;
vector3_dif(c, a, b);
float height = vector3_length(c);
c[0] /= height;
c[1] /= height;
c[2] /= height;
float cross[3];
vector3_cross(cross, c, z);
float cos0 = vector3_dot(c, z);
float sin0 = acos(cos0);
float m[16], im[16], t[16], tmpm[16];
matrix44_rotate_from_axis_sincos(m, sin0, cos0, cross[0], cross[1], cross[2]);
matrix44_translate(t, a[0], b[0], c[0]);
matrix44_mult(tmpm, t, m);
matrix44_inverse(im, tmpm, m, tmp);
MotoRay r;
moto_ray_set_transformed(&r, self, im);
*dist = 1;
return 1;
}
void vector3_test()
{
vector3 zero = {0,0,0};
vector3 one = {1,1,1};
vector3 y = {0,1,0};
vector3 half = {0.5,0.5,0.5};
vector3 a;
vector3_invert(&a, &one);
vector3_subtract(&a, &one, &a);
vector3_add(&a, &a, &one);
vector3_print(&a);
vector3_multiply(&a, &one, 0.5);
vector3_divide(&a, &a, 2);
vector3_print(&a);
vector3_reflect(&a, &one, &y);
vector3_print(&a);
vector3_scalar_sub(&a, &zero, -0.5);
vector3_scalar_add(&a, &a, 0.5);
vector3_print(&a);
vector3_cross(&a, &one, &y);
vector3_print(&a);
srand(3);
vector3_random(&a);
vector3_print(&a);
printf("%.2f %.2f\n",
vector3_dot(&half, &y), vector3_angle(&half, &y));
printf("%.2f %.2f\n",
vector3_distance(&one, &y), vector3_distancesq(&one, &y));
vector3_copy(&a, &one);
printf("%.2f %.2f\n",
vector3_length(&one), vector3_length(vector3_normalize(&a)) );
}
void vector3_normalize(Vector3* outv)
{
float length = vector3_length(outv);
if (length != 0.0f)
{
float inv = 1 / length;
outv->x *= inv;
outv->y *= inv;
outv->z *= inv;
}
}
INLINE_MODE vector3* vector3_normalize(vector3 *a)
{
v3float normalizeLength;
normalizeLength = vector3_length(a);
if(normalizeLength <= EPSILON)
{
printf("cannot normalize degenerate vector3\n");
return a;
}
vector3_divide(a, a, normalizeLength);
return a;
}
void moto_object_node_set_rotate_order(MotoObjectNode *self, MotoRotateOrder order)
{
gfloat to_target[3], eye[3];
gfloat loc_pos[] = {0, 0, 0};
gfloat *matrix = moto_object_node_get_matrix(self, TRUE);
point3_transform(eye, matrix, loc_pos);
vector3_dif(to_target, self->priv->target, eye);
gfloat dist = vector3_length(to_target);
gfloat az[] = {0, 0, 1};
gfloat n[3];
vector3_transform(n, matrix, az);
vector3_mult(to_target, n, -dist);
vector3_copy(self->priv->target, eye);
point3_move(self->priv->target, to_target, 1);
moto_node_set_param_enum((MotoNode *)self, "ro", order);
}
static void update_enemies_path()
{
vector3_t v;
u32b_t i;
// Update position of each enemy that has "started"
for(i=0; i<State->nenemies; ) {
// Move enemy towards next path point
if( (State->enemies[i].start_time < State->time) && (State->enemies[i].path != State->path) ) {
// Build vector poiting toward waypoint
vector3_sub_vector(&State->enemies[i].path->position, &State->enemies[i].position, &v);
// If waypoint is in reach, simply jump there and assign next node
if( vector3_length(&v) < State->enemies[i].speed*BASE_SPEED ) {
vector3_copy(&State->enemies[i].path->position, &State->enemies[i].position);
State->enemies[i].path = State->enemies[i].path->next;
// Move on to next entry
i++;
} else {
// Just move towards it
vector3_normalize(&v, &v);
vector3_mult_scalar(&v,&v,State->enemies[i].speed*BASE_SPEED);
vector3_add_vector(&State->enemies[i].position, &v, &State->enemies[i].position);
// Move on to next entry
i++;
}
} else if( State->enemies[i].path == State->path ) {
// Enemy reached last path node!
// Remove points from mana.
State->player.mana -= State->enemies[i].health*0.1;
// Notify GUI of the player hit.
gui_game_event_hit(State->time,&State->enemies[i]);
// Kill it (will move last entry into current); retest current.
enemy_kill_enemy(i, State->enemies, &State->nenemies, State->senemies);
} else {
// Move on to next entry.
i++;
}
}
}
double find_moid( const ELEMENTS *elem1, const ELEMENTS *elem2,
double *barbee_style_delta_v)
{
double mat1[3][3], mat2[3][3], xform_matrix[3][3];
const double identity_matrix[3][3] = {
{ 1., 0., 0.},
{ 0., 1., 0.},
{ 0., 0., 1.} };
double least_dist_squared = 10000.;
int i, j;
if( elem1->ecc > elem2->ecc)
{
const ELEMENTS *tptr = elem1;
elem1 = elem2;
elem2 = tptr;
}
fill_matrix( mat1, elem1);
fill_matrix( mat2, elem2);
for( i = 0; i < 3; i++)
for( j = 0; j < 3; j++)
xform_matrix[j][i] = dot_prod( mat1[j], mat2[i]);
for( i = 0; i < N_STEPS; i++)
{
double vect1[3], vect2[3], dist_squared = 0.;
double deriv1[3], deriv2[3], r1, r2;
double true_anomaly2 = 2. * PI * (double)i / (double)N_STEPS;
double delta_true1, delta_true2;
int loop_count = 0, solution_found = 0;
#if ((__GNUC__ * 100) + __GNUC_MINOR__) >= 406
#pragma GCC diagnostic push /* see comments above */
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
double true_anomaly1;
#pragma GCC diagnostic pop
#else
double true_anomaly1 = 0.;
#endif
do
{
r2 = compute_posn_and_derivative( elem2, true_anomaly2, xform_matrix,
vect2, deriv2);
if( !loop_count)
true_anomaly1 = atan2( vect2[1], vect2[0]);
r1 = compute_posn_and_derivative( elem1, true_anomaly1, identity_matrix,
vect1, deriv1);
for( j = 0; j < 3; j++)
vect1[j] -= vect2[j];
compute_improvement( vect1, deriv1, deriv2, &delta_true1, &delta_true2);
true_anomaly1 += delta_true1;
true_anomaly2 -= delta_true2;
if( fabs( delta_true1) < 5. * PI / N_STEPS)
if( fabs( delta_true2) < 5. * PI / N_STEPS)
{
for( j = 0; j < 3; j++)
vect1[j] += delta_true1 * deriv1[j] + delta_true2 * deriv2[j];
solution_found = 1;
}
loop_count++;
// debug_printf( " i = %3d; loop %d; %f\n",
// i, loop_count, sqrt( dot_prod( vect1, vect1)));
}
while( solution_found && loop_count < 5);
dist_squared = dot_prod( vect1, vect1);
if( dist_squared < least_dist_squared)
{
least_dist_squared = dist_squared;
if( barbee_style_delta_v)
{
double delta_v[3];
set_true_velocity( deriv1, r1, elem1->q, elem1->major_axis);
set_true_velocity( deriv2, r2, elem2->q, elem2->major_axis);
for( j = 0; j < 3; j++)
delta_v[j] = deriv1[j] - deriv2[j];
*barbee_style_delta_v = vector3_length( delta_v); /* in AU/day */
*barbee_style_delta_v *= AU_IN_KM / seconds_per_day;
}
}
#ifdef TEST_VERSION
printf( "%3d %c%8.6f%8.2f%8.2f%8.2f%8.2f%15f%15f\n", i,
(solution_found ? '*' : ' '),
sqrt( dot_prod( vect1, vect1)),
true_anomaly1 * 180. / PI,
true_anomaly2 * 180. / PI,
true_anomaly_to_eccentric( true_anomaly1, elem1->ecc) * 180. / PI,
true_anomaly_to_eccentric( true_anomaly2, elem2->ecc) * 180. / PI,
dot_prod( vect1, deriv1),
dot_prod( vect1, deriv2));
// printf( "%3d%15f%15f%15f%15f%15f\n", i, x, y,
// vect[0], vect[1], vect[2]);
#endif
}
return( sqrt( least_dist_squared));
}
INLINE_MODE v3float vector3_angle(const vector3 *a, const vector3 *b)
{
return acos(vector3_dot(a,b) / vector3_length(a) / vector3_length(b));
}
double testLength1(const Vector3& a)
{
return vector3_length(a);
}
