void Camera::CameraUpdateMatrix() // 更新相机中缓存的变换矩阵
{
Matrix4 mmove = Matrix4::getTrans(-position.x, -position.y, -position.z);
if (Type == CAMERA_TYPE_ELUER)
{
Matrix4 mRot = Matrix4(Quaternion::Euler(-eulerAngles.x * kPi / 180, -eulerAngles.y * kPi / 180, -eulerAngles.z * kPi / 180));
MatrixCamera = mmove * mRot;
}
else if (Type == CAMERA_TYPE_UVN)
{
if (UVNTargetNeedCompute)
{
// 欧拉角度求注视向量
float sin_phi, cos_phi;
sinCos(&sin_phi, &cos_phi, eulerAngles.y * kPi / 180);
float sin_theta, cos_theta;
sinCos(&sin_theta, &cos_theta, eulerAngles.z * kPi / 180);
float r = sin_phi;
UVNTarget.x = cos_theta * r;
UVNTarget.y = sin_theta * r;
UVNTarget.z = cos_phi;
}
// 定义临时的UVN(未归一化)
Vector4 u, v, n;
// 求N
n = UVNTarget - position;
n.w = 1;
// 设置V
v = V;
// 应为N和V可以组成一个平面，所以可求法向量U
u = Vector4::crossProduct(v, n);
// 因为V和N可能不垂直，所以反求V，使得V和U、N都垂直
v = Vector4::crossProduct(n, u);
// UVN归一
U = u.normalized();
V = v.normalized();
N = n.normalized();
// UVN变换矩阵
Matrix4 muvn = {
U.x, V.x, N.x, 0,
U.y, V.y, N.y, 0,
U.z, V.z, N.z, 0,
0, 0, 0, 1
};
MatrixCamera = mmove * muvn;
}
}
void Quaternion::setToRotateInertialToObject(const EulerAngles& orientation)
{
float sp, sb, sh;
float cp, cb, ch;
sinCos(&sp, &cp, orientation.pitch * 0.5f);
sinCos(&sb, &cb, orientation.bank * 0.5f);
sinCos(&sh, &ch, orientation.heading * 0.5f);
w = ch*cp*cb + sh*sp*sb;
x = -ch*sp*cb - sh*cp*sb;
y = ch*sp*sb - sh*cp*cb;
z = sh*sp*cb - ch*cp*sb;
}
void Matrix4x3::setupRotate(int axis, float theta)
{
float s, c;
sinCos(&s, &c, theta);
switch (axis)
{
case 1:
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = c; m23 = s;
m31 = 0.0f; m32 = -s; m33 = c;
break;
case 2:
m11 = c; m12 = 0.0f; m13 = -s;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = s; m32 = 0.0f; m33 = c;
break;
case 3:
m11 = c; m12 = s; m13 = 0.0f;
m21 = -s; m22 = c; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
break;
default:
assert(false);
}
tx = ty = tz = 0.0f;
}
void Matrix4x3::setupRotate(const Vector3& axis, float theta)
{
assert(fabs(axis*axis - 1.0f) < 0.01f);
float s, c;
sinCos(&s, &c, theta);
float a = 1.0f - c;
float ax = a*axis.x;
float ay = a*axis.y;
float az = a*axis.z;
m11 = ax*axis.x + c;
m12 = ax*axis.y + axis.z*s;
m13 = ax*axis.z - axis.y*s;
m21 = ay*axis.x - axis.z*s;
m22 = ay*axis.y + c;
m23 = ay*axis.z + axis.x*s;
m31 = az*axis.x + axis.y*s;
m32 = az*axis.y - axis.x*s;
m33 = az*axis.z + c;
tx = ty = tz = 0.0f;
}
void Quaternion::setToRotateInertialToObject(const EulerAngles& orientation)
{
// Compute sine and cosine of the half angles
float sp, sb, sh;
float cp, cb, ch;
sinCos(&sp, &cp, orientation.pitch * 0.5f);
sinCos(&sb, &cb, orientation.bank * 0.5f);
sinCos(&sh, &ch, orientation.heading * 0.5f);
// Compute values
w = ch*cp*cb + sh*sp*sb;
x = -ch*sp*cb - sh*cp*sb;
y = ch*sp*sb - sh*cb*cp;
z = sh*sp*cb - ch*cp*sb;
}
/// \param theta Specifies the angle of rotation.
void Matrix4x3::setupRotateZ(float theta) {
// Get sin and cosine of rotation angle
float s, c;
sinCos(&s, &c, theta);
m11 = c; m12 = s; m13 = 0.0f;
m21 = -s; m22 = c; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
tx = ty = tz = 0.0f;
}
//==============================================================================
void PerspectiveFrustum::recalculate()
{
// Planes
//
F32 c, s; // cos & sine
sinCos(getPi<F32>() + m_fovX / 2.0, s, c);
// right
m_planesL[PlaneType::RIGHT] = Plane(Vec4(c, 0.0, s, 0.0), 0.0);
// left
m_planesL[PlaneType::LEFT] = Plane(Vec4(-c, 0.0, s, 0.0), 0.0);
sinCos((getPi<F32>() + m_fovY) * 0.5, s, c);
// bottom
m_planesL[PlaneType::BOTTOM] = Plane(Vec4(0.0, s, c, 0.0), 0.0);
// top
m_planesL[PlaneType::TOP] = Plane(Vec4(0.0, -s, c, 0.0), 0.0);
// near
m_planesL[PlaneType::NEAR] = Plane(Vec4(0.0, 0.0, -1.0, 0.0), m_near);
// far
m_planesL[PlaneType::FAR] = Plane(Vec4(0.0, 0.0, 1.0, 0.0), -m_far);
// Points
//
// This came from unprojecting. It works, don't touch it
F32 x = m_far * tan(m_fovY / 2.0) * m_fovX / m_fovY;
F32 y = tan(m_fovY / 2.0) * m_far;
F32 z = -m_far;
m_pointsL[0] = Vec4(x, y, z, 0.0); // top right
m_pointsL[1] = Vec4(-x, y, z, 0.0); // top left
m_pointsL[2] = Vec4(-x, -y, z, 0.0); // bot left
m_pointsL[3] = Vec4(x, -y, z, 0.0); // bot right
}
void Quaternion::set(float angle, const Vector3& axis)
{
if(isZero(axis.x) && isZero(axis.y) && isZero(axis.z))
{
*this = IDENTITY;
}
else
{
Vector3 normAxis = axis.normalized();
Vector2 sincos = sinCos(angle * 0.5f * DEGTORAD);
w = sincos.y;
x = normAxis.x * sincos.x;
y = normAxis.y * sincos.x;
z = normAxis.z * sincos.x;
}
}
/// \param axis Specifies the axis of rotation.
/// \param theta Specifies the angle of rotation.
void Matrix4x3::setupRotate(const Vector3 &axis, float theta) {
// Quick sanity check to make sure they passed in a unit vector
// to specify the axis
assert(fabs(axis*axis - 1.0f) < .01f);
// Get sin and cosine of rotation angle
float s, c;
sinCos(&s, &c, theta);
// Compute 1 - cos(theta) and some common subexpressions
float a = 1.0f - c;
float ax = a * axis.x;
float ay = a * axis.y;
float az = a * axis.z;
// Set the matrix elements. There is still a little more
// opportunity for optimization due to the many common
// subexpressions. We'll let the compiler handle that...
m11 = ax*axis.x + c;
m12 = ax*axis.y + axis.z*s;
m13 = ax*axis.z - axis.y*s;
m21 = ay*axis.x - axis.z*s;
m22 = ay*axis.y + c;
m23 = ay*axis.z + axis.x*s;
m31 = az*axis.x + axis.y*s;
m32 = az*axis.y - axis.x*s;
m33 = az*axis.z + c;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
void CInertialFrame::GetEarthRotationSinCos (void) {
UpdateEarthRotation ();
sinCos(sin_aE, cos_aE, aE);
}