static Math::Vector3d directionToVector(float pitch, float heading) {
Math::Vector3d v;
float radHeading = Common::deg2rad(heading);
float radPitch = Common::deg2rad(pitch);
v.setValue(0, cos(radPitch) * cos(radHeading));
v.setValue(1, sin(radPitch));
v.setValue(2, cos(radPitch) * sin(radHeading));
return v;
}
void FloorFace::computePointHeight(Math::Vector3d &point) const {
// Compute the barycentric coordinates of the point in the triangle
float area = 1.0 / 2.0
* (-_vertices[1].y() * _vertices[2].x()
+ _vertices[0].y() * (-_vertices[1].x() + _vertices[2].x())
+ _vertices[0].x() * (_vertices[1].y() - _vertices[2].y())
+ _vertices[1].x() * _vertices[2].y());
float s = (_vertices[0].y() * _vertices[2].x() - _vertices[0].x() * _vertices[2].y()
+ (_vertices[2].y() - _vertices[0].y()) * point.x()
+ (_vertices[0].x() - _vertices[2].x()) * point.y())
/ (2.0 * area);
float t = (_vertices[0].x() * _vertices[1].y() - _vertices[0].y() * _vertices[1].x()
+ (_vertices[0].y() - _vertices[1].y()) * point.x()
+ (_vertices[1].x() - _vertices[0].x()) * point.y())
/ (2.0 * area);
// Compute the Z coordinate of the point
float pointZ = (1.0 - s - t) * _vertices[0].z() + s * _vertices[1].z() + t * _vertices[2].z();
point.setValue(2, pointZ);
}
