void Capsule::Transform(const float4x4 &transform)
{
	assume(transform.HasUniformScale());
	assume(transform.IsColOrthogonal3());
	l.Transform(transform);
	r *= transform.Col3(0).Length(); // Scale the radius.
}

void Sphere::Transform(const float4x4 &transform)
{
	assume(transform.HasUniformScale());
	assume(!transform.ContainsProjection());
	pos = transform.MulPos(pos);
	r *= transform.Col3(0).Length();
}

void Circle::Transform(const float4x4 &transform)
{
	assume(transform.HasUniformScale());
	assume(transform.IsColOrthogonal3());
	pos = transform.MulPos(pos);
	normal = transform.MulDir(normal).Normalized();
	r *= transform.Col3(0).Length(); // Scale the radius of the circle.
}

void AABB::TransformAsAABB(const float4x4 &transform)
{
	assume(transform.IsColOrthogonal3());
	assume(transform.HasUniformScale());
	assume(transform.Row(3).Equals(0,0,0,1));

	AABBTransformAsAABB(*this, transform);
}

void AABB::TransformAsAABB(const float4x4 &transform)
{
	assume(transform.IsColOrthogonal3());
	assume(transform.HasUniformScale());
	assume(transform.Row(3).Equals(0,0,0,1));

#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SIMD)
	AABBTransformAsAABB_SIMD(*this, transform);
#else
	AABBTransformAsAABB(*this, transform);
#endif
}

void Quat::Set(const float4x4 &m)
{
	assume(m.IsColOrthogonal3());
	assume(m.HasUnitaryScale());
	assume(!m.HasNegativeScale());
	assume(m.Row(3).Equals(0,0,0,1));
	SetQuatFrom(*this, m);

#ifdef MATH_ASSERT_CORRECTNESS
	// Test that the conversion float3x3->Quat->float3x3 is correct.
	mathassert(this->ToFloat3x3().Equals(m.Float3x3Part(), 0.01f));
#endif
}

float4x4 operator *(const float4x4 &lhs, const TranslateOp &rhs)
{
	float4x4 r = lhs;
	r.SetTranslatePart(lhs.TransformPos(rhs.Offset()));

	// Our optimized form of multiplication must be the same as this.
	mathassert(r.Equals(lhs * rhs.ToFloat4x4()));
	return r;
}

float4x4 operator *(const TranslateOp &lhs, const float4x4 &rhs)
{
	// This function is based on the optimized assumption that the last row of rhs is [0,0,0,1].
	// If this does not hold and you are hitting the check below, explicitly cast TranslateOp lhs to float4x4 before multiplication!
	assume(rhs.Row(3).Equals(0.f, 0.f, 0.f, 1.f));

	float4x4 r = rhs;
	r.SetTranslatePart(r.TranslatePart() + DIR_TO_FLOAT3(lhs.Offset()));
	return r;
}

Aabb operator * (const float4x4 &tfm, const Aabb &a)
{
  Aabb result = { tfm.getTranslation(), tfm.getTranslation() };

  for (int i = 1; i < 4; ++i)
  {
    for (int j = 1; j < 4; ++ j)
    {
      float e = tfm(i,j) * a.min[j - 1];
      float f = tfm(i,j) * a.max[j - 1];
      if (e < f)
      {
        result.min[i - 1] += e;
        result.max[i - 1] += f;
      }
      else
      {
        result.min[i - 1] += f;
        result.max[i - 1] += e;
      }
    }
  }
  return result;
}

void OBB::Transform(const float4x4 &transform)
{
    assume(transform.IsOrthogonal3());
    OBBTransform(*this, transform);
}

void Frustum::Transform(const float4x4 &transform)
{
	assume(transform.Row(3).Equals(0,0,0,1));
	Transform(transform.Float3x4Part());
}

void OBB::SetFrom(const AABB &aabb, const float4x4 &transform)
{
	assume(transform.Row(3).Equals(0,0,0,1));
	OBBSetFrom(*this, aabb, transform.Float3x4Part());
}

void Ray::Transform(const float4x4 &transform)
{
    pos = transform.TransformPos(pos);
    dir = transform.TransformDir(dir);
}

Ray operator *(const float4x4 &transform, const Ray &ray)
{
    return Ray(transform.MulPos(ray.pos), transform.MulDir(ray.dir));
}

Ray operator *(const float4x4 &transform, const Ray &ray)
{
	assume(transform.IsInvertible());
	return Ray(transform.MulPos(ray.pos), transform.MulDir(ray.dir).Normalized());
}

transform::transform(const float4x4 &m)
  : m_xfrm(m), m_inv(m.inverse())
{}

void Polyhedron::Transform(const float4x4 &transform)
{
	for(size_t i = 0; i < v.size(); ++i)
		v[i] = transform.MulPos(v[i]); ///\todo Add float4x4::BatchTransformPos.
}

void Triangle::Transform(const float4x4 &transform)
{
	a = transform.MulPos(a);
	b = transform.MulPos(b);
	c = transform.MulPos(c);
}

void Plane::Transform(const float4x4 &transform)
{
    assume(transform.Row(3).Equals(float4(0,0,0,1)));
    Transform(transform.Float3x4Part());
}

void Line::Transform(const float4x4 &transform)
{
    pos = transform.MulPos(pos);
    dir = transform.MulDir(dir);
}