void float3x4::Decompose(float3 &translate, float3x3 &rotate, float3 &scale) const
{
assume(this->IsColOrthogonal());
translate = Col(3);
rotate = RotatePart();
scale.x = rotate.Col(0).Length();
scale.y = rotate.Col(1).Length();
scale.z = rotate.Col(2).Length();
assume(!EqualAbs(scale.x, 0));
assume(!EqualAbs(scale.y, 0));
assume(!EqualAbs(scale.z, 0));
rotate.ScaleCol(0, 1.f / scale.x);
rotate.ScaleCol(1, 1.f / scale.y);
rotate.ScaleCol(2, 1.f / scale.z);
// Test that composing back yields the original float3x4.
assume(float3x4::FromTRS(translate, rotate, scale).Equals(*this, 0.1f));
}
float3x3 float3x3::operator *(const float3x3 &rhs) const
{
float3x3 r;
const float *c0 = rhs.ptr();
const float *c1 = rhs.ptr() + 1;
const float *c2 = rhs.ptr() + 2;
r[0][0] = DOT3STRIDED(v[0], c0, 3);
r[0][1] = DOT3STRIDED(v[0], c1, 3);
r[0][2] = DOT3STRIDED(v[0], c2, 3);
r[1][0] = DOT3STRIDED(v[1], c0, 3);
r[1][1] = DOT3STRIDED(v[1], c1, 3);
r[1][2] = DOT3STRIDED(v[1], c2, 3);
r[2][0] = DOT3STRIDED(v[2], c0, 3);
r[2][1] = DOT3STRIDED(v[2], c1, 3);
r[2][2] = DOT3STRIDED(v[2], c2, 3);
return r;
}
void Frustum::Transform(const float3x3 &transform)
{
assume(transform.HasUniformScale());
pos = transform * pos;
front = transform * front;
float scaleFactor = front.Normalize();
up = (transform * up).Normalized();
nearPlaneDistance *= scaleFactor;
farPlaneDistance *= scaleFactor;
if (type == OrthographicFrustum)
{
orthographicWidth *= scaleFactor;
orthographicHeight *= scaleFactor;
}
}
float3x4 float3x4::operator *(const float3x3 &rhs) const
{
///\todo SSE.
float3x4 r;
const float *c0 = rhs.ptr();
const float *c1 = rhs.ptr() + 1;
const float *c2 = rhs.ptr() + 2;
r[0][0] = DOT3STRIDED(v[0], c0, 3);
r[0][1] = DOT3STRIDED(v[0], c1, 3);
r[0][2] = DOT3STRIDED(v[0], c2, 3);
r[0][3] = v[0][3];
r[1][0] = DOT3STRIDED(v[1], c0, 3);
r[1][1] = DOT3STRIDED(v[1], c1, 3);
r[1][2] = DOT3STRIDED(v[1], c2, 3);
r[1][3] = v[1][3];
r[2][0] = DOT3STRIDED(v[2], c0, 3);
r[2][1] = DOT3STRIDED(v[2], c1, 3);
r[2][2] = DOT3STRIDED(v[2], c2, 3);
r[2][3] = v[2][3];
return r;
}
void OBB::SetFrom(const AABB &aabb, const float3x3 &transform)
{
assume(transform.IsOrthogonal());
OBBSetFrom(*this, aabb, transform);
}
void OBB::Transform(const float3x3 &transform)
{
assume(transform.IsOrthogonal());
OBBTransform(*this, transform);
}
void Capsule::Transform(const float3x3 &transform)
{
assume(transform.HasUniformScale());
l.Transform(transform);
r *= transform.Col(0).Length(); // Scale the radius.
}
/// Applies a transformation to this line.
void Ray::Transform(const float3x3 &transform)
{
pos = transform.Transform(pos);
dir = transform.Transform(dir);
}
Ray operator *(const float3x3 &transform, const Ray &ray)
{
assume(transform.IsInvertible());
return Ray(transform * ray.pos, (transform * ray.dir).Normalized());
}
void float3x3::Set(const float3x3 &rhs)
{
Set(rhs.ptr());
}
float4 operator *(const float4 &lhs, const float3x3 &rhs)
{
assume(lhs.IsWZeroOrOne());
return float4(rhs.TransformLeft(lhs.xyz()), lhs.w);
}
float3 operator *(const float3 &lhs, const float3x3 &rhs)
{
return rhs.TransformLeft(lhs);
}
void Triangle::Transform(const float3x3 &transform)
{
transform.BatchTransform(&a, 3);
}
void Polyhedron::Transform(const float3x3 &transform)
{
if (!v.empty())
transform.BatchTransform(&v[0], (int)v.size());
}
void Sphere::Transform(const float3x3 &transform)
{
assume(transform.HasUniformScale());
pos = transform * pos;
r *= transform.Col(0).Length();
}
void Plane::Transform(const float3x3 &transform)
{
float3x3 it = transform.InverseTransposed(); ///\todo Could optimize the inverse here by assuming orthogonality or orthonormality.
normal = it * normal;
}
void Polygon::Transform(const float3x3 &transform)
{
if (!p.empty())
transform.BatchTransform(&p[0], (int)p.size());
}
Ray operator *(const float3x3 &transform, const Ray &ray)
{
assume(transform.IsInvertible(1e-6f)); // Tundra: use smaller epsilon in order to prevent assumption spam when raycasting to non-uniformly scaled objects.
return Ray(transform * ray.pos, (transform * ray.dir).Normalized());
}
