Vector3D SphericalPlanet::geodeticSurfaceNormal(const Angle& latitude,
const Angle& longitude) const {
const double cosLatitude = latitude.cosinus();
return Vector3D(cosLatitude * longitude.cosinus(),
cosLatitude * longitude.sinus(),
latitude.sinus());
}
Vector3D Vector3D::rotateAroundAxis(const Vector3D& axis,
const Angle& theta) const {
const double u = axis._x;
const double v = axis._y;
const double w = axis._z;
const double cosTheta = theta.cosinus();
const double sinTheta = theta.sinus();
const double ms = axis.squaredLength();
const double m = IMathUtils::instance()->sqrt(ms);
return Vector3D(
((u * (u * _x + v * _y + w * _z)) +
(((_x * (v * v + w * w)) - (u * (v * _y + w * _z))) * cosTheta) +
(m * ((-w * _y) + (v * _z)) * sinTheta)) / ms,
((v * (u * _x + v * _y + w * _z)) +
(((_y * (u * u + w * w)) - (v * (u * _x + w * _z))) * cosTheta) +
(m * ((w * _x) - (u * _z)) * sinTheta)) / ms,
((w * (u * _x + v * _y + w * _z)) +
(((_z * (u * u + v * v)) - (w * (u * _x + v * _y))) * cosTheta) +
(m * (-(v * _x) + (u * _y)) * sinTheta)) / ms
);
}
MutableMatrix44D MutableMatrix44D::createRotationMatrix(const Angle& angle,
const Vector3D& axis) {
const Vector3D a = axis.normalized();
const double c = angle.cosinus();
const double s = angle.sinus();
return MutableMatrix44D(a._x * a._x * (1 - c) + c, a._x * a._y * (1 - c) + a._z * s, a._x * a._z * (1 - c) - a._y * s, 0,
a._y * a._x * (1 - c) - a._z * s, a._y * a._y * (1 - c) + c, a._y * a._z * (1 - c) + a._x * s, 0,
a._x * a._z * (1 - c) + a._y * s, a._y * a._z * (1 - c) - a._x * s, a._z * a._z * (1 - c) + c, 0,
0, 0, 0, 1);
}