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