double Vector_angle(const Vector vec0, const Vector vec1) { double scalp; scalp = Vector_dot(vec0, vec1); double lenp; lenp = (vec0.len * vec1.len); assert(lenp >= 0); if (Util_is_zero(lenp)) { return 0.0; } if (scalp != scalp) { Util_runtime_error("Vector_angle: Nan scalp"); } if ( Util_equal(scalp, lenp) || Util_equal(scalp, -lenp) || Util_equal(-scalp, lenp) ) { return 0.0; } double div; div = scalp / lenp; if (fabs(div - 1.0) < EPS || fabs(div + 1.0) < EPS) return 0.0; if (!Util_in_range_strict(-1.0, 1.0, div)) { Util_runtime_error("Vector_angle: Outside acos range"); } return acos(div); }
int Vector3f_dot (Vector3f *v, float *result, Vector3f *right) { CHECK_VECTOR3F(v); CHECK_VECTOR3F(right); return Vector_dot (v, result, right); }
int EulerAngles_dot (EulerAngles *e, float *result, EulerAngles *right) { CHECK_EULERANGLES(e); CHECK_EULERANGLES(right); return Vector_dot (e, result, right); }
void ray_reflect( Ray* result, const Ray* r, const Point* intersect, const Vector* norm) { result->o.x = intersect->x; result->o.y = intersect->y; result->o.z = intersect->z; double dot_norm_ray = Vector_dot(norm, &r->d); double norm_mag_squared = Vector_mag(norm) * Vector_mag(norm); double double_proj_mag = 2 * dot_norm_ray / norm_mag_squared; result->d.x = r->d.x - double_proj_mag*norm->x; result->d.y = r->d.y - double_proj_mag*norm->y; result->d.z = r->d.z - double_proj_mag*norm->z; }
void Quaternion_multiply(Quaternion * quaternion1, Quaternion quaternion2) { Vector vector1, vector2, cross; float angle; vector1 = Quaternion_toVector(*quaternion1); vector2 = Quaternion_toVector(quaternion2); angle = (quaternion1->w * quaternion2.w) - Vector_dot(vector1, vector2); cross = Vector_cross(vector1, vector2); vector1.x *= quaternion2.w; vector1.y *= quaternion2.w; vector1.z *= quaternion2.w; vector2.x *= quaternion1->w; vector2.y *= quaternion1->w; vector2.z *= quaternion1->w; quaternion1->x = vector1.x + vector2.x + cross.x; quaternion1->y = vector1.y + vector2.y + cross.y; quaternion1->z = vector1.z + vector2.z + cross.z; quaternion1->w = angle; }
double Vector_norm(const Vector v) { return sqrt(Vector_dot(v, v)); }