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; }