int sound_play_at_looped(sound* s, vec3 pos, vec3 cam_pos, vec3 cam_dir, int loops) {
int c = sound_play_looped(s, loops);
const float HEARING = 5;
float distance = vec3_dist(pos, cam_pos);
Uint8 dist_val = (Uint8)clamp(distance * HEARING, 0, 255);
const float DEGREES = 57.2957795;
vec3 to_position = vec3_normalize(vec3_sub(pos, cam_pos));
vec3 to_forward = vec3_normalize(cam_dir);
float angle = acos(vec3_dot(to_position, to_forward));
Sint16 angle_val = DEGREES * angle;
Mix_SetPosition(c, angle_val, dist_val);
return c;
}
//----------------------------------------------------------------------------------------------------------------------------------------
void OneNebuRayon::Ini() {
vec3_set(vec, 2.f*Random1() -1.f, 1.f*Random1() -.5f, 2.f*Random1() -1.f);
vec3_normalize(vec);
dist = NEBU_RAYON * Random1() * 2.f + BIGPART_SIZE;
taille = RAY_SIZE_MIN + Random1()*RAY_SIZE_RND;
totalTime = timeToGo = RAY_LIFE_MIN + Random1()*RAY_LIFE_RND;
}
vec3_t dir_for_pixel(camera_t *camera, uint32_t x, uint32_t y) {
unsigned int w, h;
w = camera->w;
h = camera->h;
vec3_t v = vec3(((float) x / w - 0.5), 0, -((float) y / h - 0.5));
return vec3_normalize(vec3_add(v, camera->dir));
}
void quat_axis_angle(quat_t *q, const vec3_t *v, float angle)
{
float s, c;
sincosf(angle/2, &s, &c);
q->w = c;
q->v = *v;
vec3_normalize(&q->v);
vec3_scale(&q->v, s);
}
mat3 terrain_axis(terrain* ter, vec2 position) {
float offset = terrain_height(ter, position);
float offset_x = terrain_height(ter, vec2_add(position, vec2_new(1,0)));
float offset_y = terrain_height(ter, vec2_add(position, vec2_new(0,1)));
vec3 pos = vec3_new(position.x+0, offset, position.y+0);
vec3 pos_xv = vec3_new(position.x+1, offset_x, position.y+0);
vec3 pos_yv = vec3_new(position.x+0, offset_y, position.y+1);
vec3 tangent = vec3_normalize(vec3_sub(pos_xv, pos));
vec3 binorm = vec3_normalize(vec3_sub(pos_yv, pos));
vec3 normal = vec3_cross(binorm, tangent);
return mat3_new(
tangent.x, tangent.y, tangent.z,
normal.x, normal.y, normal.z,
binorm.x, binorm.y, binorm.z);
}
void MeshData::generateNormals() {
int attr, len;
vec3 nf, e1, e2, *p0, *p1, *p2;
float *cur, *dst, *normals;
attr = getAttributeIndex("position");
if (attr < 0) {
#ifndef PLG_RELEASE
dprintf(2, "error: can't generateNormals() without position attribute\n");
#endif
return;
}
len = getVertexCount() * 3;
normals = (float *) malloc(len * sizeof(float));
dst = normals;
cur = getVertex(attr, 0);
while (dst < normals + len) {
p0 = (vec3 *) &cur[0],
p1 = (vec3 *) &cur[3];
p2 = (vec3 *) &cur[6];
vec3_sub(&e1, p1, p2);
vec3_sub(&e2, p2, p0);
vec3_normalize(&e1);
vec3_normalize(&e2);
vec3_cross(&nf, &e1, &e2);
memcpy(dst + 0, &nf, sizeof(vec3));
memcpy(dst + 3, &nf, sizeof(vec3));
memcpy(dst + 6, &nf, sizeof(vec3));
cur += 9;
dst += 9;
}
addNormals(normals, len);
}
static void set_ray_direction(t_ray *ray, const t_camera *camera, float increment_x, float increment_y)
{
t_vec3 dest;
vec3_copy(&dest, &camera->viewplane.upleft);
dest.x += increment_x;
dest.y -= increment_y;
vec3_copy(&ray->direction, &dest);
vec3_sub(&ray->direction, &ray->origin);
vec3_normalize(&ray->direction);
}
static void get_lum(double *col, t_scene *s, t_intersect *inter, int m)
{
t_vec3 l;
t_vec3 r;
double ln;
int i;
double is;
double id;
double dist;
double rv;
double tmp;
t_ray ray;
t_intersect shadow;
i = -1;
if (s->lights[m].light.type == DIRECTIONNAL)
{
while (++i < 3)
{
ln = dot_vec3(inter->norm, s->lights[m].light.dir);
col[i] = MAX(ln, 0) * s->lights[m].light.color.argb[i] / 255.0 *
s->lights[m].light.power;
}
return ;
}
l = sub_vec3(s->lights[m].pos, inter->pos);
dist = vec3_len(l);
l = div_vec3(l, dist);
dist -= s->lights[m].light.radius;
ln = dot_vec3(l, inter->norm);
ln = MAX(ln, 0.0);
r = vec3_normalize(sub_vec3(mult_vec3(inter->norm, 2 * ln), l));
is = s->lights[m].light.power / dist;
id = s->lights[m].light.radius * is;
rv = -dot_vec3(r, inter->dir);
rv = MAX(rv, 0.0);
ray.pos = add_vec3(inter->pos, mult_vec3(inter->norm, 0.00001));
ray.dir = l;
ray.env = NULL;
scene_intersect(s, &ray, &shadow);
if (shadow.dist < dist - 0.0001)
return ;
while (++i < 3)
{
tmp = inter->mat->diffuse * MAX(ln, 0) * id *
s->lights[m].light.color.argb[i] / 255.0;
col[i] += MAX(tmp, 0);
tmp = inter->mat->specular * pow(rv, inter->mat->shininess)
* is * s->lights[m].light.color.argb[i] / 255.0;
col[i] += MAX(tmp, 0);
}
}
void keyboard(unsigned char key, int x, int y)
{
float direction[3];
float rotation[16];
mat4_identity(rotation);
switch (key) {
case 'q':
vec3_subtract(center, eye, direction);
vec3_normalize(direction, direction);
vec3_add(eye, direction, eye);
break;
case 'e':
vec3_subtract(center, eye, direction);
vec3_normalize(direction, direction);
vec3_subtract(eye, direction, eye);
break;
case 'a':
mat4_rotateY(rotation, 0.1f, rotation);
mat4_multiply(rotation, model, model);
break;
case 'd':
mat4_rotateY(rotation, -0.1f, rotation);
mat4_multiply(rotation, model, model);
break;
case 'w':
mat4_rotateX(rotation, 0.1f, rotation);
mat4_multiply(rotation, model, model);
break;
case 's':
mat4_rotateX(rotation, -0.1f, rotation);
mat4_multiply(rotation, model, model);
break;
case 27:
exit(0);
default:
break;
}
}
t_intersect get_intersect_tore(t_obj *obj, t_ray *ray)
{
t_intersect inter;
t_vec3 u;
inter.dir = ray->dir;
inter.mat = obj->mat;
if (!check_box(obj, ray))
return (inter);
get_dist_tore(ray, &inter, obj);
if (inter.dist < 0.0 || inter.dist == NOT_A_SOLUTION)
{
inter.dist = -1.0;
return (inter);
}
inter.pos = add_vec3(mult_vec3(ray->dir, inter.dist), ray->pos);
u = sub_vec3(inter.pos, obj->pos);
u.z = 0.0;
u = mult_vec3(vec3_normalize(u), obj->torus.radius_hole);
u = add_vec3(u, obj->pos);
inter.norm = vec3_normalize(sub_vec3(inter.pos, u));
return (inter);
}
t_vec3 bump_normal(t_obj *obj, t_ray *ray)
{
t_vec3 normal;
t_vec3 tangent;
t_vec3 binormal;
t_vec3 bump;
t_vec3 c[2];
normal = obj->normal;
bump = texture_mapping(obj, obj->mat.texture.bump, ray->hit);
bump = vec3_sub(vec3_fmul(bump, 2), vec3(1, 1, 1));
c[0] = vec3_cross(normal, vec3(0, 0, 1));
c[1] = vec3_cross(normal, vec3(0, 1, 0));
tangent = (vec3_magnitude(c[0]) > vec3_magnitude(c[1]) ? c[0] : c[1]);
tangent = vec3_sub(tangent, vec3_fmul(normal, vec3_dot(tangent, normal)));
vec3_normalize(&tangent);
binormal = vec3_norm(vec3_cross(normal, tangent));
normal.x = vec3_dot(bump, vec3(tangent.x, binormal.x, normal.x));
normal.y = vec3_dot(bump, vec3(tangent.y, binormal.y, normal.y));
normal.z = vec3_dot(bump, vec3(tangent.z, binormal.z, normal.z));
vec3_normalize(&normal);
return (normal);
}
/* TODO/FIXME - finish */
void matrix_4x4_lookat(math_matrix_4x4 *out,
vec3_t eye,
vec3_t center,
vec3_t up)
{
vec3_t s, t, f;
vec3_copy(&f[0], center);
vec3_subtract(&f[0], eye);
vec3_normalize(&f[0]);
vec3_cross(&s[0], &f[0], up);
vec3_normalize(&s[0]);
vec3_cross(&t[0], &s[0], f);
memset(out, 0, sizeof(*out));
MAT_ELEM_4X4(*out, 0, 0) = s[0];
MAT_ELEM_4X4(*out, 0, 1) = t[0];
MAT_ELEM_4X4(*out, 0, 2) = -f[0];
MAT_ELEM_4X4(*out, 1, 0) = s[1];
MAT_ELEM_4X4(*out, 1, 1) = t[1];
MAT_ELEM_4X4(*out, 1, 2) = -f[1];
MAT_ELEM_4X4(*out, 2, 0) = s[2];
MAT_ELEM_4X4(*out, 2, 1) = t[2];
MAT_ELEM_4X4(*out, 2, 2) = -f[2];
MAT_ELEM_4X4(*out, 3, 3) = 1.f;
#if 0
mat4x4_translate_in_place(m, -eye[0], -eye[1], -eye[2]);
#endif
}
void matrix_4x4_lookat(math_matrix_4x4 *out,
vec3_t eye,
vec3_t center,
vec3_t up)
{
vec3_t zaxis; /* the "forward" vector */
vec3_t xaxis; /* the "right" vector */
vec3_t yaxis; /* the "up" vector */
vec3_copy(&zaxis[0], center);
vec3_subtract(&zaxis[0], eye);
vec3_normalize(&zaxis[0]);
vec3_cross(&xaxis[0], &zaxis[0], up);
vec3_normalize(&xaxis[0]);
vec3_cross(&yaxis[0], &xaxis[0], zaxis);
MAT_ELEM_4X4(*out, 0, 0) = xaxis[0];
MAT_ELEM_4X4(*out, 0, 1) = yaxis[0];
MAT_ELEM_4X4(*out, 0, 2) = -zaxis[0];
MAT_ELEM_4X4(*out, 0, 3) = 0.0;
MAT_ELEM_4X4(*out, 1, 0) = xaxis[1];
MAT_ELEM_4X4(*out, 1, 1) = yaxis[1];
MAT_ELEM_4X4(*out, 1, 2) = -zaxis[1];
MAT_ELEM_4X4(*out, 1, 3) = 0.0f;
MAT_ELEM_4X4(*out, 2, 0) = xaxis[2];
MAT_ELEM_4X4(*out, 2, 1) = yaxis[2];
MAT_ELEM_4X4(*out, 2, 2) = -zaxis[2];
MAT_ELEM_4X4(*out, 2, 3) = 0.0f;
MAT_ELEM_4X4(*out, 3, 0) = -(xaxis[0] * eye[0] + xaxis[1] * eye[1] + xaxis[2] * eye[2]);
MAT_ELEM_4X4(*out, 3, 1) = -(yaxis[0] * eye[0] + yaxis[1] * eye[1] + yaxis[2] * eye[2]);
MAT_ELEM_4X4(*out, 3, 2) = -(zaxis[0] * eye[0] + zaxis[1] * eye[1] + zaxis[2] * eye[2]);
MAT_ELEM_4X4(*out, 3, 3) = 1.f;
}
void camera_control_orbit(camera* c, SDL_Event e) {
float a1 = 0;
float a2 = 0;
vec3 axis;
vec3 translation = c->target;
c->position = vec3_sub(c->position, translation);
c->target = vec3_sub(c->target, translation);
switch(e.type) {
case SDL_MOUSEMOTION:
if (e.motion.state & SDL_BUTTON(1)) {
a1 = e.motion.xrel * -0.005;
a2 = e.motion.yrel * 0.005;
c->position = mat3_mul_vec3(mat3_rotation_y( a1 ), c->position );
axis = vec3_normalize(vec3_cross( vec3_sub(c->position, c->target) , vec3_new(0,1,0) ));
c->position = mat3_mul_vec3(mat3_rotation_axis_angle(axis, a2 ), c->position );
}
break;
case SDL_MOUSEBUTTONDOWN:
if (e.button.button == SDL_BUTTON_WHEELUP) {
c->position = vec3_sub(c->position, vec3_normalize(c->position));
}
if (e.button.button == SDL_BUTTON_WHEELDOWN) {
c->position = vec3_add(c->position, vec3_normalize(c->position));
}
break;
}
c->position = vec3_add(c->position, translation);
c->target = vec3_add(c->target, translation);
}
void camera_control_freecam(camera* c, float timestep) {
Uint8* kbstate = SDL_GetKeyState(NULL);
if (kbstate[SDLK_w] || kbstate[SDLK_s]) {
vec3 cam_dir = vec3_normalize(vec3_sub(c->target, c->position));
const float speed = 100 * timestep;
if (kbstate[SDLK_w]) {
c->position = vec3_add(c->position, vec3_mul(cam_dir, speed));
}
if (kbstate[SDLK_s]) {
c->position = vec3_sub(c->position, vec3_mul(cam_dir, speed));
}
c->target = vec3_add(c->position, cam_dir);
}
int mouse_x, mouse_y;
Uint8 mstate = SDL_GetRelativeMouseState(&mouse_x, &mouse_y);
if (mstate & SDL_BUTTON(1)) {
float a1 = -(float)mouse_x * 0.005;
float a2 = (float)mouse_y * 0.005;
vec3 cam_dir = vec3_normalize(vec3_sub(c->target, c->position));
cam_dir.y += -a2;
vec3 side_dir = vec3_normalize(vec3_cross(cam_dir, vec3_new(0,1,0)));
cam_dir = vec3_add(cam_dir, vec3_mul(side_dir, -a1));
cam_dir = vec3_normalize(cam_dir);
c->target = vec3_add(c->position, cam_dir);
}
}
collision point_collide_ellipsoid(vec3 p, vec3 v, ellipsoid e) {
p = vec3_sub(p, e.center);
p = vec3_div_vec3(p, e.radiuses);
v = vec3_div_vec3(v, e.radiuses);
collision c = point_collide_sphere(p, v, sphere_unit());
c.norm = vec3_normalize(vec3_div_vec3(c.norm, e.radiuses));
c.point = vec3_mul_vec3(c.point, e.radiuses);
c.point = vec3_add(c.point, e.center);
return c;
}
void GFX_look_at( vec3 *eye, vec3 *center, vec3 *up )
{
vec3 f,
s,
u;
mat4 mat;
mat4_identity( &mat );
vec3_diff( &f, center, eye );
vec3_normalize( &f, &f );
vec3_cross( &s, &f, up );
vec3_normalize( &s, &s );
vec3_cross( &u, &s, &f );
mat.m[ 0 ].x = s.x;
mat.m[ 1 ].x = s.y;
mat.m[ 2 ].x = s.z;
mat.m[ 0 ].y = u.x;
mat.m[ 1 ].y = u.y;
mat.m[ 2 ].y = u.z;
mat.m[ 0 ].z = -f.x;
mat.m[ 1 ].z = -f.y;
mat.m[ 2 ].z = -f.z;
GFX_multiply_matrix( &mat );
GFX_translate( -eye->x, -eye->y, -eye->z );
}
collision sphere_collide_sphere(sphere s, vec3 v, sphere s0) {
//if (unlikely(!sphere_outside_sphere(s, s0))) { error("Collision Sphere Inside Sphere!"); }
vec3 o = vec3_sub(s.center, s0.center);
float A = vec3_dot(v, v);
float B = 2 * vec3_dot(v, o);
float C = vec3_dot(o, o) - ((s.radius + s0.radius) * (s.radius + s0.radius));
float t0, t1, t;
if (!quadratic(A, B, C, &t0, &t1)) { return collision_none(); }
if (between_or(t0, 0, 1) && between_or(t1, 0, 1)) { t = min(t0, t1); }
else if (between_or(t0, 0, 1)) { t = t0; }
else if (between_or(t1, 0, 1)) { t = t1; }
else { return collision_none(); }
vec3 proj = vec3_add(s.center, vec3_mul(v, t));
vec3 twrd = vec3_normalize(vec3_sub(s0.center, proj));
vec3 p = vec3_add(proj, vec3_mul(twrd, s.radius));
return collision_new(t, p, vec3_normalize(vec3_sub(s.center, p)));
}
collision ellipsoid_collide_point(ellipsoid e, vec3 v, vec3 p) {
p = vec3_sub(p, e.center);
p = vec3_div_vec3(p, e.radiuses);
v = vec3_div_vec3(v, e.radiuses);
collision c = sphere_collide_point(sphere_unit(), v, p);
c.norm = vec3_normalize(vec3_div_vec3(c.norm, e.radiuses));
c.point = vec3_mul_vec3(c.point, e.radiuses);
c.point = vec3_add(c.point, e.center);
return c;
}
void mat4_rotate( mat4 *dst, mat4 *m, vec4 *v )
{
float s = sinf( v->w * DEG_TO_RAD ),
c = cosf( v->w * DEG_TO_RAD ),
xx,
yy,
zz,
xy,
yz,
zx,
xs,
ys,
zs,
c1;
mat4 mat;
vec3 t = { v->x, v->y, v->z };
mat4_identity( &mat );
if( !v->w || !vec3_normalize( &t, &t ) ) return;
xx = t.x * t.x;
yy = t.y * t.y;
zz = t.z * t.z;
xy = t.x * t.y;
yz = t.y * t.z;
zx = t.z * t.x;
xs = t.x * s;
ys = t.y * s;
zs = t.z * s;
c1 = 1.0f - c;
mat.m[ 0 ].x = ( c1 * xx ) + c;
mat.m[ 1 ].x = ( c1 * xy ) - zs;
mat.m[ 2 ].x = ( c1 * zx ) + ys;
mat.m[ 0 ].y = ( c1 * xy ) + zs;
mat.m[ 1 ].y = ( c1 * yy ) + c;
mat.m[ 2 ].y = ( c1 * yz ) - xs;
mat.m[ 0 ].z = ( c1 * zx ) - ys;
mat.m[ 1 ].z = ( c1 * yz ) + xs;
mat.m[ 2 ].z = ( c1 * zz ) + c;
mat4_multiply_mat4( m, m, &mat );
}
/* External functions
*/
Game* create_game(void)
{
int ii;
Game* G = (Game*)calloc(1, sizeof(Game));
G->timer = create_timer();
G->graphics = create_graphics();
G->ui = create_ui(G->graphics);
/* Set up camera */
G->camera = transform_zero;
G->camera.orientation = quat_from_euler(0, -0.75f * kPi, 0);
G->camera.position.x = 4.0f;
G->camera.position.y = 2;
G->camera.position.z = 7.5f;
/* Load scene */
reset_timer(G->timer);
G->scene = create_scene("lightHouse.obj");
G->sun_light.position = vec3_create(-4.0f, 5.0f, 2.0f);
G->sun_light.color = vec3_create(1, 1, 1);
G->sun_light.size = 35.0f;
G->lights[0].color = vec3_create(1, 0, 0);
G->lights[1].color = vec3_create(1, 1, 0);
G->lights[2].color = vec3_create(0, 1, 0);
G->lights[3].color = vec3_create(1, 0, 1);
G->lights[4].color = vec3_create(0, 0, 1);
G->lights[5].color = vec3_create(0, 1, 1);
for(ii=0;ii<NUM_LIGHTS;++ii) {
float x = (20.0f/NUM_LIGHTS) * ii - 8.0f;
G->lights[ii].color = vec3_create(_rand_float(), _rand_float(), _rand_float());
G->lights[ii].color = vec3_normalize(G->lights[ii].color);
if(ii % 2)
G->lights[ii].position = vec3_create(x, _rand_float()*3 + 2.0f, 0.0f);
else
G->lights[ii].position = vec3_create(0.0f, _rand_float()*3 + 2.0f, x);
G->lights[ii].size = 5;
}
get_model(G->scene, 3)->material->specular_color = vec3_create(0.5f, 0.5f, 0.5f);
get_model(G->scene, 3)->material->specular_coefficient = 1.0f;
G->dynamic_lights = 1;
reset_timer(G->timer);
return G;
}
void templateAppAccelerometer( float x, float y, float z )
{
vec3 tmp = { x, y, z };
vec3_normalize( &tmp, &tmp );
accelerometer.x = tmp.x + 0.35f;
#ifndef __IPHONE_4_0
accelerometer.y = tmp.y + 0.35f;
#else
accelerometer.y = tmp.y;
#endif
}
static t_rgb compute_gradient(double *grad, t_obj *obj)
{
t_vec3 color;
double diffx;
double diffy;
double scale;
scale = obj->mat.texture.normal_strength;
diffx = grad[1] - grad[2];
diffy = grad[0] - grad[3];
color.x = vec3_norm(vec3(1, diffx * scale, 0)).y;
color.y = vec3_norm(vec3(1, diffy * scale, 0)).y;
color.z = sqrt(1 - ft_clampf(color.x * color.x + color.y * color.y, 0, 1));
vec3_normalize(&color);
return (vec3_to_rgb(vec3_add(vec3_fmul(color, 0.5), vec3(0.5, 0.5, 0.5))));
}
static void raycast_light(t_hit *hit, unsigned depth)
{
t_lstiter it;
t_ray ray;
t_light *light;
int raycast_result;
t_vec3 lightness;
t_hit sub_hit;
if (depth == 0)
return ;
vec3_set(&lightness, 0, 0, 0);
init_iter(&it, rt.scene->lights, increasing);
while (lst_iterator_next(&it))
{
light = (t_light*)it.data;
vec3_copy(&ray.origin, &hit->position);
vec3_copy(&ray.direction, &light->position);
vec3_sub(&ray.direction, &hit->position);
vec3_normalize(&ray.direction);
ray.origin.x += ray.direction.x * RC_SHADOW_SHIFT;
ray.origin.y += ray.direction.y * RC_SHADOW_SHIFT;
ray.origin.z += ray.direction.z * RC_SHADOW_SHIFT;
raycast_result = raycast(&ray, &sub_hit, depth - 1, NULL);
if (sub_hit.object != NULL)
{
// ray bounce
}
else
{
vec3_add(&lightness, &light->color);
}
}
vec3_div(&lightness, rt.scene->lights->size);
vec3_add(&lightness, &rt.scene->ambient_light);
color_clamp(&lightness);
hit->color.x *= lightness.x;
hit->color.y *= lightness.y;
hit->color.z *= lightness.z;
}
/**
* Component of the support function for a sphere collider.
**/
static void
object_support_component_sphere(object_t *object,
float direction[3], float out[3])
{
float trans[16];
vec3_normalize(direction, direction);
vec3_scale(direction, direction, object->r);
object_get_transform_mat(object, trans);
out[0] = trans[3];
out[1] = trans[7];
out[2] = trans[11];
vec3_add(direction, out, out);
}
collision point_collide_sphere(vec3 p, vec3 v, sphere s) {
vec3 o = vec3_sub(p, s.center);
float A = vec3_dot(v, v);
float B = 2 * vec3_dot(v, o);
float C = vec3_dot(o, o) - (s.radius * s.radius);
float t0, t1, t;
if (!quadratic(A, B, C, &t0, &t1)) { return collision_none(); }
if (between_or(t0, 0, 1) && between_or(t1, 0, 1)) { t = min(t0, t1); }
else if (between_or(t0, 0, 1)) { t = t0; }
else if (between_or(t1, 0, 1)) { t = t1; }
else { return collision_none(); }
return collision_new(t, p, vec3_normalize(vec3_sub(p, s.center)));
}
void move_entity( OBJMESH *objmesh,
NAVIGATIONPATHDATA *navigationpathdata,
int *next_point,
float speed ) {
objmesh->location.z =
navigationpathdata->path_point_array[ *next_point ].z = 0.0f;
float distance = vec3_dist( &objmesh->location,
&navigationpathdata->path_point_array[ *next_point ] );
if( distance < 0.1f ) {
++*next_point;
if( *next_point == ( navigationpathdata->path_point_count + 1 ) ) {
*next_point = -1;
}
}
if( *next_point != -1 ) {
vec3 direction;
vec3_diff( &direction,
&navigationpathdata->path_point_array[ *next_point ],
&objmesh->location );
vec3_normalize( &direction,
&direction );
objmesh->btrigidbody->setLinearVelocity( btVector3( direction.x * speed,
direction.y * speed,
0.0f ) );
objmesh->btrigidbody->setActivationState( ACTIVE_TAG );
}
else {
objmesh->btrigidbody->setActivationState( WANTS_DEACTIVATION );
navigationpathdata->path_point_count = 0;
}
}
collision sphere_collide_point(sphere s, vec3 v, vec3 p) {
//if (unlikely(!point_outside_sphere(s, p))) { error("Collision Sphere Inside Mesh Vertex!"); }
vec3 o = vec3_sub(s.center, p);
float A = vec3_dot(v, v);
float B = 2 * vec3_dot(v, o);
float C = vec3_dot(o, o) - (s.radius * s.radius);
float t0, t1, t;
if (!quadratic(A, B, C, &t0, &t1)) { return collision_none(); }
if (between_or(t0, 0, 1) && between_or(t1, 0, 1)) { t = min(t0, t1); }
else if (between_or(t0, 0, 1)) { t = t0; }
else if (between_or(t1, 0, 1)) { t = t1; }
else { return collision_none(); }
return collision_new(t, p, vec3_normalize(vec3_sub(s.center, p)));
}
collision ellipsoid_collide_mesh(ellipsoid e, vec3 v, cmesh* m, mat4 world, mat3 world_normal) {
world.xw -= e.center.x;
world.yw -= e.center.y;
world.zw -= e.center.z;
mat3 space = mat3_scale(vec3_div_vec3(vec3_one(), e.radiuses));
mat3 space_inv = mat3_scale(e.radiuses);
v = mat3_mul_vec3(space, v);
collision c = sphere_collide_mesh_space(sphere_unit(), v, m, world, world_normal, space, mat3_transpose(space_inv));
c.point = mat3_mul_vec3(space_inv, c.point);
c.point = vec3_add(c.point, e.center);
c.norm = vec3_normalize(mat3_mul_vec3(space, c.norm));
return c;
}
void calculate_normal(vec3_t normal, int edge, size_t x, size_t y, size_t z, float offset, scan_data *data)
{
vec3 v1;
vec3 v2;
get_normal((vec3_t) &v1, (x + cube_vertices[cube_edge_connections[edge][0]][0]),
(y + cube_vertices[cube_edge_connections[edge][0]][1]),
(z + cube_vertices[cube_edge_connections[edge][0]][2]),
data);
get_normal((vec3_t) &v2, (x + cube_vertices[cube_edge_connections[edge][1]][0]),
(y + cube_vertices[cube_edge_connections[edge][1]][1]),
(z + cube_vertices[cube_edge_connections[edge][1]][2]),
data);
normal[0] = v1[0] + (v2[0] - v1[0]) * offset;
normal[1] = v1[1] + (v2[1] - v1[1]) * offset;
normal[2] = v1[2] + (v2[2] - v1[2]) * offset;
vec3_normalize(normal, normal);
}