int moto_ray_intersect_plane(MotoRay *self,
MotoIntersection *intersection,
float point[3], float normal[3])
{
float dif[3];
vector3_dif(dif, point, self->pos);
float numer = vector3_dot(normal, dif);
float denom = vector3_dot(normal, self->dir);
if(fabs(denom) < MICRO) /* parallel */
{
return 0;
}
float t = numer/denom;
if(t < MICRO)
{
return 0;
}
intersection->hits[0].dist = t;
intersection->hits[0].is_entering = 1;
vector3_copy(intersection->hits[0].point, self->pos);
point3_move(intersection->hits[0].point, self->dir, t);
vector3_copy(intersection->hits[0].normal, normal);
intersection->hits_num = 1;
return 1;
}
vector2 v3tov2(vector3 vec)
{
return (vector2) { .x = vec.x, .y = vec.y };
}
vector2 doProjectPoint(projector tr, vector3 p)
{
transformer_matrix tm = { .matrix = {{ 0.0f }} };
for (int i = 0; i<2; ++i) {
float t = vector3_dot(vector3_norm(
vector3_sub(tr.dir,
vector3_mul(tr.axis[1-i],
vector3_dot(tr.axis[1-i],
tr.dir)))),
vector3_cross(tr.axis[0], tr.axis[1]));
float k = sqrtf(1.0f - sqr(t))/t;
for(int j = 0; j<3; ++j) {
tm.matrix[i][j] = vector3_dot(axis[j], tr.axis[i]);
tm.matrix[i][j] += sqrtf(1.0f - sqr(tm.matrix[i][j])) * k *
vector3_dot(axis[j], vector3_cross(tr.axis[0], tr.axis[1]));
}
tm.matrix[i][3] = 0;
}
return v3tov2(doTransformMatrix(tm, vector3_sub(p, tr.base)));
}
int moto_ray_intersect_sphere_2_check(MotoRay *self,
float origin[3], float square_radius)
{
float dst[3];
vector3_dif(dst, self->pos, origin);
float B = vector3_dot(dst, self->dir);
float C = vector3_dot(dst, dst) - square_radius;
float D = B*B - C;
if(D > MICRO)
{
float sqrtD = sqrt(D);
if((sqrtD - B) > MICRO)
return 1;
if((-sqrtD - B) > MICRO)
return 1;
}
else if(D >= 0 && D < MICRO)
{
if(-B < MICRO)
return 1;
}
return 0;
}
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;
}
/// \brief Returns the point at which \p line intersects \p plane, or an undefined value if there is no intersection.
inline DoubleVector3 line_intersect_plane(const DoubleLine& line, const Plane3& plane)
{
return line.origin + vector3_scaled(
line.direction,
-plane3_distance_to_point(plane, line.origin)
/ vector3_dot(line.direction, plane.normal())
);
}
/// \brief Keep the value of \p infinity as small as possible to improve precision in Winding_Clip.
void Winding_createInfinite(FixedWinding& winding, const Plane3& plane, double infinity)
{
double max = -infinity;
int x = -1;
for (int i=0 ; i<3; i++)
{
double d = fabs(plane.normal()[i]);
if (d > max)
{
x = i;
max = d;
}
}
if(x == -1)
{
globalErrorStream() << "invalid plane\n";
return;
}
DoubleVector3 vup = g_vector3_identity;
switch (x)
{
case 0:
case 1:
vup[2] = 1;
break;
case 2:
vup[0] = 1;
break;
}
vector3_add(vup, vector3_scaled(plane.normal(), -vector3_dot(vup, plane.normal())));
vector3_normalise(vup);
DoubleVector3 org = vector3_scaled(plane.normal(), plane.dist());
DoubleVector3 vright = vector3_cross(vup, plane.normal());
vector3_scale(vup, infinity);
vector3_scale(vright, infinity);
// project a really big axis aligned box onto the plane
DoubleLine r1, r2, r3, r4;
r1.origin = vector3_added(vector3_subtracted(org, vright), vup);
r1.direction = vector3_normalised(vright);
winding.push_back(FixedWindingVertex(r1.origin, r1, c_brush_maxFaces));
r2.origin = vector3_added(vector3_added(org, vright), vup);
r2.direction = vector3_normalised(vector3_negated(vup));
winding.push_back(FixedWindingVertex(r2.origin, r2, c_brush_maxFaces));
r3.origin = vector3_subtracted(vector3_added(org, vright), vup);
r3.direction = vector3_normalised(vector3_negated(vright));
winding.push_back(FixedWindingVertex(r3.origin, r3, c_brush_maxFaces));
r4.origin = vector3_subtracted(vector3_subtracted(org, vright), vup);
r4.direction = vector3_normalised(vup);
winding.push_back(FixedWindingVertex(r4.origin, r4, c_brush_maxFaces));
}
int moto_ray_intersect_sphere_2_dist(MotoRay *self,
float *dist, float origin[3], float square_radius)
{
float dst[3], dd[2];
vector3_dif(dst, self->pos, origin);
float B = vector3_dot(dst, self->dir);
float C = vector3_dot(dst, dst) - square_radius;
float D = B*B - C;
if(D > MICRO)
{
float sqrtD = sqrt(D);
int num = 0;
dd[0] = sqrtD - B;
if(dd[0] > MICRO)
{
*dist = dd[0];
num++;
}
dd[1] = -sqrtD - B;
if(dd[1] > MICRO)
{
*dist = dd[1];
num++;
}
if( ! num)
return 0;
if(num == 2)
*dist = min(dd[0], dd[1]);
return 1;
}
else if(D >= 0 && D < MICRO)
{
*dist = -B;
if(*dist < MICRO)
return 1;
}
return 0;
}
/* TODO: Needs to be optimized? */
int moto_ray_intersect_triangle(MotoRay *self,
MotoIntersection *intersection,
float A[3], float B[3], float C[3])
{
float tmp;
float normal[3], v1[3], v2[3];
vector3_dif(v1, B, A);
vector3_dif(v2, C, A);
vector3_cross(normal, v1, v2);
vector3_normalize(normal, tmp);
if( ! moto_ray_intersect_plane(self, intersection, A, normal))
return 0;
float n1[3], n2[3], n3[3], oA[3], oB[3], oC[3];
vector3_dif(oA, A, self->pos);
vector3_dif(oB, B, self->pos);
vector3_dif(oC, C, self->pos);
if(vector3_dot(self->dir, normal) < 0) /* faceforward */
{
vector3_cross(n1, oC, oA);
vector3_normalize(n1, tmp);
if(vector3_dot(n1, self->dir) > 0)
return 0;
vector3_cross(n2, oB, oC);
vector3_normalize(n2, tmp);
if(vector3_dot(n2, self->dir) > 0)
return 0;
vector3_cross(n3, oA, oB);
vector3_normalize(n3, tmp);
if(vector3_dot(n3, self->dir) > 0)
return 0;
}
else
{
vector3_cross(n1, oA, oC);
vector3_normalize(n1, tmp);
if(vector3_dot(n1, self->dir) > 0)
return 0;
vector3_cross(n2, oC, oB);
vector3_normalize(n2, tmp);
if(vector3_dot(n2, self->dir) > 0)
return 0;
vector3_cross(n3, oB, oA);
vector3_normalize(n3, tmp);
if(vector3_dot(n3, self->dir) > 0)
return 0;
}
return 1;
}
INLINE_MODE vector3* vector3_reflect(vector3 *dest, const vector3 *incoming, const vector3 *normal)
{
v3float dp;
dp = 2*vector3_dot(normal, incoming);
dest->x = incoming->x - dp*normal->x;
dest->y = incoming->y - dp*normal->y;
dest->z = incoming->z - dp*normal->z;
return dest;
}
int moto_ray_intersect_plane_check(MotoRay *self,
float point[3], float normal[3])
{
float dif[3];
vector3_dif(dif, point, self->pos);
float numer = vector3_dot(normal, dif);
float denom = vector3_dot(normal, self->dir);
if(fabs(denom) < MICRO) /* parallel */
{
return 0;
}
if(numer/denom < MICRO)
{
return 0;
}
return 1;
}
vector3
matrix33_multiply(const vector3 lhs, const matrix33 &rhs)
{
const detail::internal_mat3 *right = reinterpret_cast<const detail::internal_mat3*>(&rhs);
std::array<float, 3> vec_data;
for(int i = 0; i < 9; i += 4)
{
const vector3 dot_vector = vector3_init(right->data[i + 0], right->data[i + 3], right->data[i + 6]);
vec_data.at(i / 3) = vector3_dot(lhs, dot_vector);
}
return vector3_init_with_array(vec_data);
}
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)) );
}
matrix33
matrix33_multiply(const matrix33 &lhs, const matrix33 &rhs)
{
const detail::internal_mat3 *left = reinterpret_cast<const detail::internal_mat3*>(&lhs);
const detail::internal_mat3 *right = reinterpret_cast<const detail::internal_mat3*>(&rhs);
matrix33 return_mat;
detail::internal_mat3 *internal_mat = reinterpret_cast<detail::internal_mat3*>(&return_mat);
for(uint32_t i = 0; i < 9; ++i)
{
//[0,1,2,3] x [0,4,8,12]
const uint32_t row = i / 3;
const uint32_t col = i % 3;
const vector3 left_vec = vector3_init(left->data[row + 0], left->data[row + 1], left->data[row + 2]);
const vector3 right_vec = vector3_init(right->data[col + 0], right->data[col + 3], right->data[col + 6]);
internal_mat->data[i] = vector3_dot(left_vec, right_vec);
}
return return_mat;
}
INLINE_MODE v3float vector3_angle(const vector3 *a, const vector3 *b)
{
return acos(vector3_dot(a,b) / vector3_length(a) / vector3_length(b));
}
double testDot1(const Vector3& a, const Vector3& b)
{
return vector3_dot(a, b);
}
inline double plane3_distance_to_point(const Plane3& plane, const Vector3& point)
{
return vector3_dot(point, plane.normal()) - plane.dist();
}
int moto_ray_intersect_sphere_2(MotoRay *self,
MotoIntersection *intersection,
float origin[3], float square_radius)
{
float tmp, dst[3];
vector3_dif(dst, self->pos, origin);
float B = vector3_dot(dst, self->dir);
float C = vector3_dot(dst, dst) - square_radius;
float D = B*B - C;
if(D > MICRO)
{
float sqrtD = sqrt(D);
intersection->hits_num = 0;
intersection->hits[0].dist = sqrtD - B;
if(intersection->hits[0].dist > MICRO)
{
vector3_copy(intersection->hits[0].point, self->pos);
point3_move(intersection->hits[0].point, self->dir, intersection->hits[0].dist);
vector3_dif(intersection->hits[0].normal, intersection->hits[0].point, origin);
vector3_normalize(intersection->hits[0].normal, tmp);
intersection->hits[0].is_entering = \
(vector3_dot(intersection->hits[0].normal, self->dir) < 0);
intersection->hits_num++;
}
intersection->hits[intersection->hits_num].dist = -sqrtD - B;
if(intersection->hits[intersection->hits_num].dist > MICRO)
{
vector3_copy(intersection->hits[intersection->hits_num].point, self->pos);
point3_move(intersection->hits[intersection->hits_num].point, self->dir,
intersection->hits[intersection->hits_num].dist);
vector3_dif(intersection->hits[intersection->hits_num].normal,
intersection->hits[intersection->hits_num].point, origin);
vector3_normalize(intersection->hits[intersection->hits_num].normal, tmp);
intersection->hits[intersection->hits_num].is_entering = \
(vector3_dot(intersection->hits[intersection->hits_num].normal, self->dir) < 0);
intersection->hits_num++;
}
return intersection->hits_num > 0;
}
else if(D >= 0 && D < MICRO)
{
intersection->hits_num = 0;
intersection->hits[0].dist = -B;
if(-B < MICRO)
{
vector3_copy(intersection->hits[0].point, self->pos);
point3_move(intersection->hits[0].point, self->dir,
intersection->hits[0].dist);
vector3_dif(intersection->hits[0].normal, intersection->hits[0].point, origin);
vector3_normalize(intersection->hits[0].normal, tmp);
intersection->hits[0].is_entering = 0;
intersection->hits_num = 1;
}
return intersection->hits_num > 0;
}
return 0;
}
