#
# File:: matrix.rb
# Description:: Matrix classes.
#
# Author:: David Muir <david.muir@rockstarnorth.com>
# Date:: 10 December 2008
#

#----------------------------------------------------------------------------
# Uses
#----------------------------------------------------------------------------
require 'pipeline/math/constants'
require 'pipeline/math/vector3'

#----------------------------------------------------------------------------
# Implementation
#----------------------------------------------------------------------------
module Pipeline
module Math

	#
	# == Description
	# The Matrix34 class represents a matrix with 3 columns and 4 rows.
	#
	# Ref: X:\jimmy\src\dev\rage\base\src\vector\matrix34_default.h.
	#
	class Matrix34
		#--------------------------------------------------------------------
		# Attributes
		#--------------------------------------------------------------------
		attr_accessor :a
		attr_accessor :b
		attr_accessor :c
		attr_accessor :d
		
		#--------------------------------------------------------------------
		# Virtual Attributes
		#--------------------------------------------------------------------
		
		#
		# Return translation component of Matrix34.
		#
		def translation
			@d
		end
		
		#
		# Set translation component of Matrix34.
		#
		def translation=( trans )
			throw ArgumentError.new( "Invalid Vector3 object (#{trans.class})." ) \
				unless ( trans.is_a?( Vector3 ) )
			@d = trans
		end
		
		#--------------------------------------------------------------------
		# Methods
		#--------------------------------------------------------------------
		
		#
		# Constructor, default arguments create an identity matrix.
		#
		def initialize( a = Vector3.new( 1.0, 0.0, 0.0 ),
						b = Vector3.new( 0.0, 1.0, 0.0 ),
						c = Vector3.new( 0.0, 0.0, 1.0 ), 
						d = Vector3.new( 0.0, 0.0, 0.0 ) )
			@a = a
			@b = b
			@c = c
			@d = d
		end
		
		#
		# Set matrix to the identity matrix.
		#
		def identity!( )
			@a = Vector3.new( 1.0, 0.0, 0.0 )
			@b = Vector3.new( 0.0, 1.0, 0.0 )
			@c = Vector3.new( 0.0, 0.0, 1.0 )
			@d = Vector3.new( 0.0, 0.0, 0.0 )
		end
		
		#
		# Set matrix to zero.
		#
		def zero!( )
			@a = Vector3.new( 0.0, 0.0, 0.0 )
			@b = Vector3.new( 0.0, 0.0, 0.0 )
			@c = Vector3.new( 0.0, 0.0, 0.0 )
			@d = Vector3.new( 0.0, 0.0, 0.0 )
		end
		
		#
		# Add a matrix to the current matrix.
		#
		def +( m )
			throw ArgumentError.new( "Invalid matrix specified (#{m.class})." ) \
				unless ( m.is_a?( Matrix34 ) )			
			@a += m.a
			@b += m.b
			@c += m.c
			@d += m.d
		end
		
		#
		# Subtract a matrix from the current matrix.
		#
		def -( m )
			throw ArgumentError.new( "Invalid matrix specified (#{m.class})." ) \
				unless ( m.is_a?( Matrix34 ) )			
			@a -= m.a
			@b -= m.b
			@c -= m.c
			@d -= m.d
		end

		#
		# Transform a Matrix34 or Vector3 by this Matrix34, returning a new 
		# Matrix34 or Vector3 object.
		#
		def *( v )
			throw ArgumentError.new( "Invalid Matrix34 or Vector3 specified (#{v.class})." ) \
				unless ( v.is_a?( Matrix34 ) or v.is_a?( Vector3 ) )
			
			if ( v.is_a?( Vector3 ) ) then
				newX = v.x * a.x + v.y * b.x + v.z * c.x + d.x;
				newY = v.x * a.y + v.y * b.y + v.z * c.y + d.y;
				newZ = v.x * a.z + v.y * b.z + v.z * c.z + d.z;
				Vector3.new( newX, newY, newZ )
				
			elsif ( v.is_a?( Matrix34 ) ) then
				
				ma = Vector3.new
				mb = Vector3.new
				mc = Vector3.new
				md = Vector3.new
				
				ma.x = @a.x*v.a.x + @a.y*v.b.x + @a.z*v.c.x;
				ma.y = @a.x*v.a.y + @a.y*v.b.y + @a.z*v.c.y;
				ma.z = @a.x*v.a.z + @a.y*v.b.z + @a.z*v.c.z;
				
				mb.x = @b.x*v.a.x + @b.y*v.b.x + @b.z*v.c.x;
				mb.y = @b.x*v.a.y + @b.y*v.b.y + @b.z*v.c.y;
				mb.z = @b.x*v.a.z + @b.y*v.b.z + @b.z*v.c.z;
				
				mc.x = @c.x*v.a.x + @c.y*v.b.x + @c.z*v.c.x;
				mc.y = @c.x*v.a.y + @c.y*v.b.y + @c.z*v.c.y;
				mc.z = @c.x*v.a.z + @c.y*v.b.z + @c.z*v.c.z;
				
				md.x = @d.x*v.a.x + @d.y*v.b.x + @d.z*v.c.x + v.d.x;
				md.y = @d.x*v.a.y + @d.y*v.b.y + @d.z*v.c.y + v.d.y;
				md.z = @d.x*v.a.z + @d.y*v.b.z + @d.z*v.c.z + v.d.z;
				
				Matrix34.new( ma, mb, mc, md )
			end
		end
		
		#
		# Return inverse of this matrix.
		#
		def inverse( )
			Matrix34::inverse( self )
		end

		#
		# Return whether the matrix can be inverted.
		#
		def invertable?( )
			# Get three of the subdeterminants.
			subDetX = Math::Determinant22( m.b.y, m.b.z, m.c.y, m.c.z );
			subDetY = Math::Determinant22( m.b.x, m.b.z, m.c.x, m.c.z );
			subDetZ = Math::Determinant22( m.b.x, m.b.y, m.c.x ,m.c.y );
			
			# Find the largest absolute value element.
			bigElement = [ [ m.a.x.abs, m.a.y.abs, m.a.z.abs ].max,
				[ m.b.x.abs, m.b.y.abs, m.b.z.abs ].max,
				[ m.c.x.abs, m.c.y.abs, m.c.z.abs ].max ].max
			
			# Get the inverse of the determinant.
			invDet = ( m.a.x * subDetX ) - ( m.a.y * subDetY ) + ( m.a.z * subDetZ );
			return ( invDet.abs > ( 3.6e-7 * bigElement ) )
		end
		
		#
		# Serialise to String.
		#
		def to_s( )
			"#{@a}\n#{@b}\n#{@c}\n#{@d}"
		end
		
		#--------------------------------------------------------------------
		# Class Methods
		#--------------------------------------------------------------------
		
		#
		# Return the identity Matrix34.
		#
		def Matrix34::identity( )
			
			Matrix34.new( Vector3.new( 1.0, 0.0, 0.0 ),
						Vector3.new( 0.0, 1.0, 0.0 ),
					Vector3.new( 0.0, 0.0, 1.0 ) )
		end
		
		#
		# Return the inverse of a Matrix34.
		#
		def Matrix34::inverse( m )
			
			# Get three of the subdeterminants.
			subDetX = Math::Determinant22( m.b.y, m.b.z, m.c.y, m.c.z );
			subDetY = Math::Determinant22( m.b.x, m.b.z, m.c.x, m.c.z );
			subDetZ = Math::Determinant22( m.b.x, m.b.y, m.c.x ,m.c.y );
	
			# Find the largest absolute value element.
			bigElement = [ [ m.a.x.abs, m.a.y.abs, m.a.z.abs ].max,
				[ m.b.x.abs, m.b.y.abs, m.b.z.abs ].max,
				[ m.c.x.abs, m.c.y.abs, m.c.z.abs ].max ].max
			
			# Get the inverse of the determinant.
			invDet = ( m.a.x * subDetX ) - ( m.a.y * subDetY ) + ( m.a.z * subDetZ );
			if ( invDet.abs > ( 3.6e-7 * bigElement ) ) then
		
				invm = Matrix34.new( )
				invDet = 1.0 / invDet;
				
				# Start making the inverse matrix.
				invm.a.x = subDetX * invDet;
				invm.b.x = -subDetY * invDet;
				invm.c.x = subDetZ * invDet;
				invm.d.x = - (m.d.x*invm.a.x + m.d.y*invm.b.x + m.d.z*invm.c.x);
				
				# Get three more subdeterminants.
				subDetX = Math::Determinant22( m.a.y,m.a.z,m.c.y,m.c.z );
				subDetY = Math::Determinant22( m.a.x,m.a.z,m.c.x,m.c.z );
				subDetZ = Math::Determinant22( m.a.x,m.a.y,m.c.x,m.c.y );
				
				# Add more terms to the inverse matrix.
				invm.a.y = -subDetX*invDet;
				invm.b.y = subDetY*invDet;
				invm.c.y = -subDetZ*invDet;
				invm.d.y = - (m.d.x*invm.a.y + m.d.y*invm.b.y + m.d.z*invm.c.y);
				
				# Get the last three subdeterminants.
				subDetX = Math::Determinant22( m.a.y,m.a.z,m.b.y,m.b.z );
				subDetY = Math::Determinant22( m.a.x,m.a.z,m.b.x,m.b.z );
				subDetZ = Math::Determinant22( m.a.x,m.a.y,m.b.x,m.b.y );
				
				# Finish making the inverse matrix.
				invm.a.z = subDetX*invDet;
				invm.b.z = -subDetY*invDet;
				invm.c.z = subDetZ*invDet;
				invm.d.z = -(m.d.x*invm.a.z + m.d.y*invm.b.z + m.d.z*invm.c.z);
				
				# Return inverted matrix (the determinant was not too close to zero).
				return ( invm )
			end
			
			# The determinant of this matrix is too close to zero to do an accurate inverse.
			Matrix34::identity( )
		end
		
		#
		# Construct a Matrix34 object from a XML node.  This should be coded
		# to handle both libXML and REXML XML node types, which it does
		# provided the Vector3::from_xml handles both libraries.
		#
		def Matrix34::from_xml( xml_node )
			a = Vector3::from_xml( xml_node.find_first( 'a' ) )
			b = Vector3::from_xml( xml_node.find_first( 'b' ) )
			c = Vector3::from_xml( xml_node.find_first( 'c' ) )
			d = Vector3::from_xml( xml_node.find_first( 'd' ) )
			Matrix34.new( a, b, c, d )
		end
		
		#
		# Construct a Matrix34 object from a Quaternion (Quat) object, 
		# representing a rotation.
		#
		def Matrix34::from_quat( quat )
			tx = quat.x * M_SQRT2
			ty = quat.y * M_SQRT2
			tz = quat.z * M_SQRT2
			tw = quat.w * M_SQRT2
			
			ma = Vector3.new
			mb = Vector3.new
			mc = Vector3.new
			md = Vector3.new

		    ma.y = tx*ty + tz*tw;
			mb.x = tx*ty - tz*tw;
			
			ma.z = tx*tz - ty*tw;
			mc.x = tx*tz + ty*tw;
			
			mb.z = ty*tz + tx*tw;
			mc.y = ty*tz - tx*tw;
			
			ty *= ty;   ## need squares along diagonal
			tz *= tz;
			tx *= tx;
			ma.x = 1.0 - (ty + tz)
			mb.y = 1.0 - (tz + tx)
			mc.z = 1.0 - (ty + tx)
			
			Matrix34.new( ma, mb, mc, md )
		end
	end
	
	#
	# Return the 2x2 matrix determinant.
	#
	def Math::Determinant22( a, b, c, d )
		( ( a * d ) - ( b * c ) )
	end
	
	#
	# Return the 3x3 matrix determinant.
	#
	def Math::Determinant33( ax, ay, az, bx, by, bz, cx, cy, cz )
		( ax*by*cz ) + ( ay*bz*cx ) + ( az*bx*cy ) - ( ax*bz*cy ) - ( ay*bx*cz ) - ( az*by*cx )
	end
	
end # Math module
end # Pipeline module

# matrix.rb