static int _llfunc_vec4_sub(lua_State *L) {
vec4 *a = (vec4*)userdata_get_or_die(L, 1);
vec4 *b = (vec4*)userdata_get_or_die(L, 2);
vec4 *r = (vec4*)userdata_get_or_new(L, 3, sizeof(vec4));
vec4_sub(a, b, r);
return 1;
}
void make_inv(t_vec4 inv[4], t_vec4 vec[4], t_vec4 faces[6], t_vec4 sign[2])
{
inv[0] = vec4_add(vec4_sub(vec4_mul(vec[1], faces[0]),
vec4_mul(vec[2], faces[1])),
vec4_mul(vec[3], faces[2]));
inv[1] = vec4_add(vec4_sub(vec4_mul(vec[0], faces[0]),
vec4_mul(vec[2], faces[3])),
vec4_mul(vec[3], faces[4]));
inv[2] = vec4_add(vec4_sub(vec4_mul(vec[0], faces[1]),
vec4_mul(vec[1], faces[3])),
vec4_mul(vec[3], faces[5]));
inv[3] = vec4_add(vec4_sub(vec4_mul(vec[0], faces[2]),
vec4_mul(vec[1], faces[4])),
vec4_mul(vec[2], faces[5]));
sign[0].s = (t_4dvec){+1, -1, +1, -1};
sign[1].s = (t_4dvec){-1, +1, -1, +1};
}
void point_calc_normal(Point *point, distance_func sdf)
{
float eps = 0.1e-4;
vec4 pos = {point->pos[0], point->pos[1], point->pos[2], 1.0};
vec4 epsX = {eps, 0.f, 0.f, 0.0f};
vec4 epsY = {0.f, eps, 0.f, 0.0f};
vec4 epsZ = {0.f, 0.f, eps, 0.0f};
vec4 ppx, psx, ppy, psy, ppz, psz;
vec4_add(ppx, pos, epsX);
vec4_add(ppy, pos, epsY);
vec4_add(ppz, pos, epsZ);
vec4_sub(psx, pos, epsX);
vec4_sub(psy, pos, epsY);
vec4_sub(psz, pos, epsZ);
point->norm[0] = sdf(ppx) - sdf(psx);
point->norm[1] = sdf(ppy) - sdf(psy);
point->norm[2] = sdf(ppz) - sdf(psz);
vec3_norm(point->norm, point->norm);
}
/* create a ray from two points [p1 -> p2] (vector4_t under the hood) */
ray_t* ray_create(ray_t *rayout, const point_t *p1, const point_t *p2)
{
/* origin of ray is point 1 */
vec4_set(&rayout->origin, (float *)p1);
/* get vector from point 1 to point 2 */
vec4_sub(&rayout->direction, p2, p1);
/* get magnitude of vector we just created */
rayout->magnitude = vec4_magnitude(&rayout->direction);
/* normalize the vector this way to prevent duplicate work */
vec4_scale(&rayout->direction, &rayout->direction, 1.0f / rayout->magnitude);
return rayout;
}
/* tests if ray intersects a given polygon */
int ray_intersect_polygon(const ray_t *ray, const polygon_t* poly,
point_t *pt, float *distance)
{
float num = -1.0f * (
vec4_dot((vector4_t *)&poly->plane, &ray->origin) +
poly->plane.F);
float den = vec4_dot((vector4_t *)&poly->plane, &ray->direction);
float w;
unsigned int i = 0; /* counter variable for loop */
double angleTotal = 0.0; /* running total of angle */
double angleTmp = 0.0; /* angle between current two vectors */
vector4_t tmp;
vector4_t tmp2;
#ifdef __SPU__
vector float vTmp;
#endif
/* if denomenator = 0, ray is parallel to plane */
if(den == 0.0f)
{ /* return no intersection */
return 0;
}
w = num / den; /* distance to intersection */
/* if w < 0, intersection point is behind ray */
if(w < 0.0f)
{ /* return no intersection */
return 0;
}
/* now w is least positive root */
/* use it to calculate where intersection point is */
vec4_add(pt, &ray->origin,
vec4_scale(&tmp, &ray->direction, w));
*distance = w; /* pass back distance to intersection */
/* at this point we at least know the ray intersects the plane.
* let's figure out if the point is actually inside the confined
* polygonal area */
for(i = 0; i < poly->nVerticies; ++i)
{
if(i == (poly->nVerticies - 1))
{ /* last vertex, compare with first */
/* calculate two vectors */
vec4_sub(&tmp, &poly->vertex[i], pt);
vec4_sub(&tmp2, &poly->vertex[0], pt);
/* find angle between them - between normal vectors, dot
* product is cos of angle between them */
#ifdef __SPU__
vTmp[0] = vec4_costheta(&tmp, &tmp2);
vTmp = _acosf4(vTmp);
angleTotal += vTmp[0];
#else
angleTmp = vec4_costheta(&tmp, &tmp2);
/* arccos to get theta */
angleTmp = acos(angleTmp);
angleTotal += angleTmp;
#endif
}
else
{
/* calculate two vectors */
vec4_sub(&tmp, &poly->vertex[i], pt);
vec4_sub(&tmp2, &poly->vertex[i+1], pt);
/* find angle between them - between normal vectors, dot
* product is cos of angle between them */
#ifdef __SPU__
vTmp[0] = vec4_costheta(&tmp, &tmp2);
vTmp = _acosf4(vTmp);
angleTotal += vTmp[0];
#else
angleTmp = vec4_costheta(&tmp, &tmp2);
/* arccos to get theta */
angleTmp = acos(angleTmp);
angleTotal += angleTmp;
#endif
}
}
/* TODO: this comparison is weak, should be refined */
if((angleTotal + .001) > (2.0 * M_PI))
{
return 1;
}
else
{
return 0;
}
}
