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

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