/HelloOpenGL/HelloOpenGL/CC3GLMatrix.m

/*
 * CC3GLMatrix.m
 *
 * $Version: cocos3d 0.5.4 (f5cd4df5048c) on 2011-04-14 $
 * Author: Bill Hollings
 * Copyright (c) 2010-2011 The Brenwill Workshop Ltd. All rights reserved.
 * http://www.brenwill.com
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 * http://en.wikipedia.org/wiki/MIT_License
 *
 * See header file CC3GLMatrix.h for full API documentation.
 */

#import "CC3GLMatrix.h"
#import "CC3Math.h"
#import "CC3Kazmath.h"


#pragma mark CC3Matrix private method declaration

@interface CC3GLMatrix (Private)
-(id) initParent;
-(id) initWithFirstElement: (GLfloat) e00 remainingElements: (va_list) args;
-(void) swap: (GLuint) idx1 with: (GLuint) idx2;
+(void) swap: (GLuint) idx1 with: (GLuint) idx2 inMatrix: (GLfloat*) aGLMatrix;
@end


#pragma mark -
#pragma mark CC3ArrayMatrix class cluster implementation class

@interface CC3GLArrayMatrix : CC3GLMatrix {
	GLfloat glArray[16];
}

@end

@implementation CC3GLArrayMatrix

-(GLfloat*) glMatrix {
	return glArray;
}

@end


#pragma mark -
#pragma mark CC3GLPointerMatrix class cluster implementation class

@interface CC3GLPointerMatrix : CC3GLMatrix {
	GLfloat* glMatrix;
}

@end

@implementation CC3GLPointerMatrix

-(GLfloat*) glMatrix {
	return glMatrix;
}

-(id) initOnGLMatrix: (GLfloat*) aGLMtx {
	if ( (self = [self initParent]) ) {
		glMatrix = aGLMtx;
	}
	return self;
}

@end


#pragma mark -
#pragma mark CC3Matrix implementation

@implementation CC3GLMatrix

@synthesize isIdentity;

/**
 * Abstract class simply returns NULL.
 * Subclasses will provide concrete access to the appropriate structure.
 */
-(GLfloat*) glMatrix {
	return NULL;
}


#pragma mark Allocation and initialization

-(id) initParent {
	return [super init];
}

-(id) init {
	if ( [self isKindOfClass: [CC3GLArrayMatrix class]] ) {
		if( (self = [self initParent]) ) {
			[self populateZero];
		}
		return self;
	} else {
		[self release];
		return [[CC3GLArrayMatrix alloc] init];
	}
}

+(id) matrix {
	if ( [self isSubclassOfClass: [CC3GLArrayMatrix class]] ) {
		return [[[self alloc] init] autorelease];
	} else {
		return [CC3GLArrayMatrix matrix];
	}
}

-(id) initIdentity {
	if ( [self isKindOfClass: [CC3GLArrayMatrix class]] ) {
		if( (self = [self initParent]) ) {
			[self populateIdentity];
		}
		return self;
	} else {
		[self release];
		return [[CC3GLArrayMatrix alloc] initIdentity];
	}
}

+(id) identity {
	if ( [self isSubclassOfClass: [CC3GLArrayMatrix class]] ) {
		return [[[self alloc] initIdentity] autorelease];
	} else {
		return [CC3GLArrayMatrix identity];
	}
}

-(id) initFromGLMatrix: (GLfloat*) aGLMtx {
	if ( [self isKindOfClass: [CC3GLArrayMatrix class]] ) {
		if( (self = [self initParent]) ) {
			[self populateFromGLMatrix: aGLMtx];
		}
		return self;
	} else {
		[self release];
		return [[CC3GLArrayMatrix alloc] initFromGLMatrix: aGLMtx];
	}
}

+(id) matrixFromGLMatrix: (GLfloat*) aGLMtx {
	if ( [self isSubclassOfClass: [CC3GLArrayMatrix class]] ) {
		return [[[self alloc] initFromGLMatrix: aGLMtx] autorelease];
	} else {
		return [CC3GLArrayMatrix matrixFromGLMatrix: aGLMtx];
	}
}

-(id) initWithFirstElement: (GLfloat) e00 remainingElements: (va_list) args {
	if ( [self isKindOfClass: [CC3GLArrayMatrix class]] ) {
		if( (self = [self initParent]) ) {
			GLfloat* p = self.glMatrix;
			*p++ = e00;
			for (int i = 1; i < 16; i++) {
				*p++ = (GLfloat)va_arg(args, double);
			}
		}
	} else {
		[self release];
		self = [[CC3GLArrayMatrix alloc] initWithFirstElement: e00 remainingElements: args];
	}
	return self;
}

-(id) initWithElements: (GLfloat) e00, ... {
	va_list args;
	va_start(args, e00);
	self = [self initWithFirstElement: e00 remainingElements: args];
	va_end(args);
	return self;
}

+(id) matrixWithElements: (GLfloat) e00, ... {
	va_list args;
	va_start(args, e00);
	CC3GLMatrix* mtx = [[CC3GLArrayMatrix alloc] initWithFirstElement: e00 remainingElements: args];
	va_end(args);
	return [mtx autorelease];
}

-(id) initOnGLMatrix: (GLfloat*) aGLMtx {
	[self release];
	return [[CC3GLPointerMatrix alloc] initOnGLMatrix: aGLMtx];
}

+(id) matrixOnGLMatrix: (GLfloat*) aGLMtx {
	if ( [self isSubclassOfClass: [CC3GLPointerMatrix class]] ) {
		return [[[self alloc] initOnGLMatrix: aGLMtx] autorelease];
	} else {
		return [CC3GLPointerMatrix matrixOnGLMatrix: aGLMtx];
	}
}

- (id) copyWithZone: (NSZone*) zone {
	return [[CC3GLArrayMatrix matrixFromGLMatrix: self.glMatrix] retain];
}

-(NSString*) description {
	GLfloat* m = self.glMatrix;
	NSMutableString* desc = [NSMutableString stringWithCapacity: 200];
	[desc appendFormat: @"\n\t[%.3f, ", m[0]];
	[desc appendFormat: @"%.3f, ", m[4]];
	[desc appendFormat: @"%.3f, ", m[8]];
	[desc appendFormat: @"%.3f,\n\t ", m[12]];
	[desc appendFormat: @"%.3f, ", m[1]];
	[desc appendFormat: @"%.3f, ", m[5]];
	[desc appendFormat: @"%.3f, ", m[9]];
	[desc appendFormat: @"%.3f,\n\t ", m[13]];
	[desc appendFormat: @"%.3f, ", m[2]];
	[desc appendFormat: @"%.3f, ", m[6]];
	[desc appendFormat: @"%.3f, ", m[10]];
	[desc appendFormat: @"%.3f,\n\t ", m[14]];
	[desc appendFormat: @"%.3f, ", m[3]];
	[desc appendFormat: @"%.3f, ", m[7]];
	[desc appendFormat: @"%.3f, ", m[11]];
	[desc appendFormat: @"%.3f]", m[15]];
	return desc;
}


#pragma mark -
#pragma mark Instance population

static const GLfloat identityContents[] = { 1.0f, 0.0f, 0.0f, 0.0f,
											0.0f, 1.0f, 0.0f, 0.0f,
											0.0f, 0.0f, 1.0f, 0.0f,
											0.0f, 0.0f, 0.0f, 1.0f };

-(void) populateFrom: (CC3GLMatrix*) aMtx {
	if (aMtx.isIdentity) {
		[self populateIdentity];
	} else {
		[self populateFromGLMatrix: aMtx.glMatrix];
	}
}

-(void) populateFromGLMatrix: (GLfloat*) aGLMtx {
	[[self class] copyMatrix: aGLMtx into: self.glMatrix];
	isIdentity = NO;
}

-(void) populateZero {
	memset(self.glMatrix, 0, 16 * sizeof(GLfloat));
	isIdentity = NO;
}

-(void) populateIdentity {
	if (!isIdentity) {
		[[self class] copyMatrix: (GLfloat*)identityContents into: self.glMatrix];
		isIdentity = YES;
	}
}

-(void) populateFromTranslation: (CC3Vector) aVector {
/*
     | 1  0  0  x |
 M = | 0  1  0  y |
     | 0  0  1  z |
     | 0  0  0  1 |
*/
	// Start with identity, then if the vector is not zero,
	// add the translation components, and mark not identity.
	[self populateIdentity];

	if (!CC3VectorsAreEqual(aVector, kCC3VectorZero)) {
		GLfloat* m = self.glMatrix;
		m[12] = aVector.x;
		m[13] = aVector.y;
		m[14] = aVector.z;
		isIdentity = NO;
	}
}

-(void) populateFromRotation: (CC3Vector) aVector {
	if (CC3VectorsAreEqual(aVector, kCC3VectorZero)) {
		[self populateIdentity];
	} else {
		CC3Vector rotRads = CC3VectorScaleUniform(aVector, DegreesToRadiansFactor);
		kmMat4RotationYXZ((kmMat4*)self.glMatrix, rotRads.x, rotRads.y, rotRads.z);
		isIdentity = NO;
	}
}

-(void) populateFromQuaternion: (CC3Vector4) aQuaternion {
	if (CC3Vector4sAreEqual(aQuaternion, kCC3Vector4QuaternionIdentity)) {
		[self populateIdentity];
	} else {
		kmMat4RotationQuaternion((kmMat4*)self.glMatrix, (kmQuaternion*)&aQuaternion);
		isIdentity = NO;
	}
}

-(void) populateFromScale: (CC3Vector) aVector {
/*
     |  x  0  0  0 |
 M = |  0  y  0  0 |
     |  0  0  z  0 |
     |  0  0  0  1 |
 */
	// Start with identity, then if the vector is not unity,
	// add the scale components, and mark not identity.
	[self populateIdentity];
	
	if (!CC3VectorsAreEqual(aVector, kCC3VectorUnitCube)) {
		GLfloat* m = self.glMatrix;
		m[0] = aVector.x;
		m[5] = aVector.y;
		m[10] = aVector.z;
		isIdentity = NO;
	}
}

-(void) populateToPointTowards: (CC3Vector) fwdDirection withUp: (CC3Vector) upDirection {
	[[self class] populate: self.glMatrix toPointTowards: fwdDirection withUp: upDirection];
	isIdentity = NO;
}

-(void) populateToLookAt: (CC3Vector) targetLocation
			   withEyeAt: (CC3Vector) eyeLocation
				  withUp: (CC3Vector) upDirection {

	[[self class] populate: self.glMatrix
				  toLookAt: targetLocation
				 withEyeAt: eyeLocation
					withUp: upDirection];
	isIdentity = NO;
}

-(void) populateFromFrustumLeft: (GLfloat) left
					   andRight: (GLfloat) right
					  andBottom: (GLfloat) bottom
						 andTop: (GLfloat) top  
						andNear: (GLfloat) near
						 andFar: (GLfloat) far {
	[[self class] populate: self.glMatrix
		   fromFrustumLeft: left andRight: right
				 andBottom: bottom andTop: top  
				   andNear: near andFar: far];
	isIdentity = NO;
}

-(void) populateOrthoFromFrustumLeft: (GLfloat) left
							andRight: (GLfloat) right
						   andBottom: (GLfloat) bottom
							  andTop: (GLfloat) top  
							 andNear: (GLfloat) near
							  andFar: (GLfloat) far {
	[[self class] populateOrtho: self.glMatrix
				fromFrustumLeft: left andRight: right
					  andBottom: bottom andTop: top  
						andNear: near andFar: far];
	isIdentity = NO;
}


#pragma mark Matrix population

+(void) copyMatrix: (GLfloat*) srcGLMatrix into: (GLfloat*) destGLMatrix {
	memcpy(destGLMatrix, srcGLMatrix, 16 * sizeof(GLfloat));
}

+(void) populate: (GLfloat*) aGLMatrix toPointTowards: (CC3Vector) fwdDirection withUp: (CC3Vector) upDirection {
/*
     |  rx  ux  -fx  0 |
 M = |  ry  uy  -fy  0 |
     |  rz  uz  -fz  0 |
     |  0   0    0   1 |
	 
	 where f is the normalized Forward vector (the direction being pointed to)
	 and u is the normalized Up vector in the rotated frame
	 and r is the normalized Right vector in the rotated frame
 */
	CC3Vector f, u, r;
	
	f = CC3VectorNormalize(fwdDirection);
	r = CC3VectorNormalize(CC3VectorCross(f, upDirection));
	u = CC3VectorCross(r, f);			// already normalized since f & r are orthonormal
	
	aGLMatrix[0]  = r.x;
	aGLMatrix[1]  = r.y;
	aGLMatrix[2]  = r.z;
	aGLMatrix[3] = 0.0;
	
	aGLMatrix[4]  = u.x;
	aGLMatrix[5]  = u.y;
	aGLMatrix[6]  = u.z;
	aGLMatrix[7] = 0.0;
	
	aGLMatrix[8]  = -f.x;
	aGLMatrix[9]  = -f.y;
	aGLMatrix[10] = -f.z;
	aGLMatrix[11] = 0.0;
	
	aGLMatrix[12]  = 0.0;
	aGLMatrix[13]  = 0.0;
	aGLMatrix[14] = 0.0;
	aGLMatrix[15] = 1.0;
}

+(void) populate: (GLfloat*) aGLMatrix
		toLookAt: (CC3Vector) targetLocation
	   withEyeAt: (CC3Vector) eyeLocation
		  withUp: (CC3Vector) upDirection {
	
	CC3Vector fwdDir = CC3VectorDifference(targetLocation, eyeLocation);
	[self populate: aGLMatrix toPointTowards: fwdDir withUp: upDirection];
	[self transpose: aGLMatrix];		
	[self translate: aGLMatrix by: CC3VectorNegate(eyeLocation)];
}

+(void) populate: (GLfloat*) aGLMatrix
 fromFrustumLeft: (GLfloat) left
		andRight: (GLfloat) right
	   andBottom: (GLfloat) bottom
		  andTop: (GLfloat) top  
		 andNear: (GLfloat) near
		  andFar: (GLfloat) far {
	
	aGLMatrix[0]  = (2.0 * near) / (right - left);
	aGLMatrix[1]  = 0.0;
	aGLMatrix[2]  = 0.0;
	aGLMatrix[3] = 0.0;
	
	aGLMatrix[4]  = 0.0;
	aGLMatrix[5]  = (2.0 * near) / (top - bottom);
	aGLMatrix[6]  = 0.0;
	aGLMatrix[7] = 0.0;
	
	aGLMatrix[8]  = (right + left) / (right - left);
	aGLMatrix[9]  = (top + bottom) / (top - bottom);
	aGLMatrix[10] = -(far + near) / (far - near);
	aGLMatrix[11] = -1.0;
	
	aGLMatrix[12]  = 0.0;
	aGLMatrix[13]  = 0.0;
	aGLMatrix[14] = -(2.0 * far * near) / (far - near);
	aGLMatrix[15] = 0.0;
}

+(void) populateOrtho: (GLfloat*) aGLMatrix
	  fromFrustumLeft: (GLfloat) left
			 andRight: (GLfloat) right
			andBottom: (GLfloat) bottom
			   andTop: (GLfloat) top  
			  andNear: (GLfloat) near
			   andFar: (GLfloat) far {
	
	aGLMatrix[0]  = 2.0 / (right - left);
	aGLMatrix[1]  = 0.0;
	aGLMatrix[2]  = 0.0;
	aGLMatrix[3] = 0.0;
	
	aGLMatrix[4]  = 0.0;
	aGLMatrix[5]  = 2.0 / (top - bottom);
	aGLMatrix[6]  = 0.0;
	aGLMatrix[7] = 0.0;

	aGLMatrix[8]  = 0.0;
	aGLMatrix[9]  = 0.0;
	aGLMatrix[10]  = -2.0 / (far - near);
	aGLMatrix[11] = 0.0;

	aGLMatrix[12]  = -(right + left) / (right - left);
	aGLMatrix[13]  = -(top + bottom) / (top - bottom);
	aGLMatrix[14] = -(far + near) / (far - near);
	aGLMatrix[15] = 1.0;
}


#pragma mark -
#pragma mark Instance accessing

 // CAUTION: This is a simple convenience utility. For speed, it does not honour
 // the isIdentity flag. It is the responsibility of the caller to deal with that flag.
-(void) swap: (GLuint) idx1 with: (GLuint) idx2 {
	[[self class] swap: idx1 with: idx2 inMatrix: self.glMatrix];
}

-(CC3Vector) extractRotation {
	return [[self class] extractRotationYXZFromMatrix: self.glMatrix];
}

-(CC3Vector4) extractQuaternion {
	return [[self class] extractQuaternionFromMatrix: self.glMatrix];
}

-(CC3Vector) extractForwardDirection {
	return [[self class] extractForwardDirectionFrom: self.glMatrix];
}

-(CC3Vector) extractUpDirection {
	return [[self class] extractUpDirectionFrom: self.glMatrix];
}

-(CC3Vector) extractRightDirection {
	return [[self class] extractRightDirectionFrom: self.glMatrix];
}


#pragma mark Matrix accessing

+(void) swap: (GLuint) idx1 with: (GLuint) idx2 inMatrix: (GLfloat*) aGLMatrix {
	GLfloat tmp = aGLMatrix[idx1];
	aGLMatrix[idx1] = aGLMatrix[idx2];
	aGLMatrix[idx2] = tmp;
}

// Assumes YXZ euler order, which is the OpenGL ES default
+(CC3Vector) extractRotationYXZFromMatrix: (GLfloat*) aGLMatrix {
/*
     |  cycz + sxsysz   czsxsy - cysz   cxsy  0 |
 M = |  cxsz            cxcz           -sx    0 |
     |  cysxsz - czsy   cyczsx + sysz   cxcy  0 |
     |  0               0               0     1 |
 
     where cA = cos(A), sA = sin(A) for A = x,y,z
 */
	GLfloat radX, radY, radZ;
	GLfloat cxsz = aGLMatrix[1];
	GLfloat cxcz = aGLMatrix[5];
	GLfloat sx = -aGLMatrix[9];
	GLfloat cxsy = aGLMatrix[8];
	GLfloat cxcy = aGLMatrix[10];
	if (sx < +1.0) {
		if (sx > -1.0) {
			radX = asin(sx);
			radY = atan2(cxsy, cxcy);
			radZ = atan2(cxsz, cxcz);
		}
		else {		// sx = -1. Not a unique solution: radZ + radY = atan2(-m01,m00).
			radX = -M_PI_2;
			radY = atan2(-aGLMatrix[4], aGLMatrix[0]);
			radZ = 0.0;
		}
	}
	else {			// sx = +1. Not a unique solution: radZ - radY = atan2(-m01,m00).
		radX = +M_PI_2;
		radY = -atan2(-aGLMatrix[4], aGLMatrix[0]);
		radZ = 0.0;
	}	
	return cc3v(RadiansToDegrees(radX), RadiansToDegrees(radY), RadiansToDegrees(radZ));
}

// Assumes ZYX euler order
+(CC3Vector) extractRotationZYXFromMatrix: (GLfloat*) aGLMatrix {
/*
     |  cycz  -cxsz + sxsycz   sxsz + cxsycz  0 |
 M = |  cysz   cxcz + sxsysz  -sxcz + cxsysz  0 |
     | -sy     sxcy            cxcy           0 |
     |  0      0               0              1 |
	 
     where cA = cos(A), sA = sin(A) for A = x,y,z
 */
	GLfloat radX, radY, radZ;
	GLfloat cycz = aGLMatrix[0];
	GLfloat cysz = aGLMatrix[1];
	GLfloat sy = -aGLMatrix[2];
	GLfloat sxcy = aGLMatrix[6];
	GLfloat cxcy = aGLMatrix[10];
	if (sy < +1.0) {
		if (sy > -1.0) {
			radY = asin(sy);
			radZ = atan2(cysz, cycz);
			radX = atan2(sxcy, cxcy);
		}
		else {		// sy = -1. Not a unique solution: radX + radZ = atan2(-m12,m11).
			radY = -M_PI_2;
			radZ = atan2(-aGLMatrix[9], aGLMatrix[5]);
			radX = 0.0;
		}
	}
	else {			// sy = +1. Not a unique solution: radX - radZ = atan2(-m12,m11).
		radY = +M_PI_2;
		radZ = -atan2(-aGLMatrix[9], aGLMatrix[5]);
		radX = 0.0;
	}	
	return cc3v(RadiansToDegrees(radX), RadiansToDegrees(radY), RadiansToDegrees(radZ));
}

+(CC3Vector4) extractQuaternionFromMatrix: (GLfloat*) aGLMatrix {
	CC3Vector4 quaternion;
	kmQuaternionRotationMatrix((kmQuaternion*)&quaternion, (kmMat4*)aGLMatrix);
	return quaternion;
}

+(CC3Vector) extractForwardDirectionFrom: (GLfloat*) aGLMatrix {
	return cc3v(-aGLMatrix[8], -aGLMatrix[9], -aGLMatrix[10]);
}

+(CC3Vector) extractUpDirectionFrom: (GLfloat*) aGLMatrix {
	return cc3v(aGLMatrix[4], aGLMatrix[5], aGLMatrix[6]);
}

+(CC3Vector) extractRightDirectionFrom: (GLfloat*) aGLMatrix {
	return cc3v(aGLMatrix[0], aGLMatrix[1], aGLMatrix[2]);
}


#pragma mark -
#pragma mark Instance transformations

-(void) translateBy: (CC3Vector) translationVector
		   rotateBy: (CC3Vector) rotationVector
			scaleBy: (CC3Vector) scaleVector {
	// if not ALL identity transforms, transform this matrix
	if ( !(CC3VectorsAreEqual(translationVector, kCC3VectorZero) &&
		   CC3VectorsAreEqual(rotationVector, kCC3VectorZero) &&
		   CC3VectorsAreEqual(scaleVector, kCC3VectorUnitCube)) ) {
		[[self class] transform: self.glMatrix
					translateBy: translationVector
					   rotateBy: rotationVector
						scaleBy: scaleVector];
		isIdentity = NO;
	}
}

-(void) translateBy: (CC3Vector) aVector {
	// Short-circuit an identity transform
	if ( !CC3VectorsAreEqual(aVector, kCC3VectorZero) ) {
		[[self class] translate: self.glMatrix by: aVector];
		isIdentity = NO;
	}
}

-(void) translateByX: (GLfloat) distance {
	// Short-circuit an identity transform
	if ( distance != 0.0f ) {
		[[self class] translate: self.glMatrix byX: distance];
		isIdentity = NO;
	}
}

-(void) translateByY: (GLfloat) distance {
	// Short-circuit an identity transform
	if ( distance != 0.0f ) {
		[[self class] translate: self.glMatrix byY: distance];
		isIdentity = NO;
	}
}

-(void) translateByZ: (GLfloat) distance {
	// Short-circuit an identity transform
	if ( distance != 0.0f ) {
		[[self class] translate: self.glMatrix byZ: distance];
		isIdentity = NO;
	}
}

-(void) rotateBy: (CC3Vector) aVector {
	// Short-circuit an identity transform
	if ( !CC3VectorsAreEqual(aVector, kCC3VectorZero) ) {
		[[self class] rotateYXZ: self.glMatrix by: aVector];
		isIdentity = NO;
	}
}

-(void) rotateByX: (GLfloat) degrees {
	// Short-circuit an identity transform
	if ( degrees != 0.0f ) {
		[[self class] rotate: self.glMatrix byX: degrees];
		isIdentity = NO;
	}
}

-(void) rotateByY: (GLfloat) degrees {
	// Short-circuit an identity transform
	if ( degrees != 0.0f ) {
		[[self class] rotate: self.glMatrix byY: degrees];
		isIdentity = NO;
	}
}

-(void) rotateByZ: (GLfloat) degrees {
	// Short-circuit an identity transform
	if ( degrees != 0.0f ) {
		[[self class] rotate: self.glMatrix byZ: degrees];
		isIdentity = NO;
	}
}

-(void) rotateByQuaternion: (CC3Vector4) aQuaternion {
	// Short-circuit an identity transform
	if ( !CC3Vector4sAreEqual(aQuaternion, kCC3Vector4QuaternionIdentity) ) {
		[[self class] rotate: self.glMatrix byQuaternion: aQuaternion];
		isIdentity = NO;
	}
}

-(void) scaleBy: (CC3Vector) aVector {
	// Short-circuit an identity transform
	if ( !CC3VectorsAreEqual(aVector, kCC3VectorUnitCube) ) {
		[[self class] scale: self.glMatrix by: aVector];
		isIdentity = NO;
	}
}

-(void) scaleByX: (GLfloat) scaleFactor {
	// Short-circuit an identity transform
	if ( scaleFactor != 1.0f ) {
		[[self class] scale: self.glMatrix byX: scaleFactor];
		isIdentity = NO;
	}
}

-(void) scaleByY: (GLfloat) scaleFactor {
	// Short-circuit an identity transform
	if ( scaleFactor != 1.0f ) {
		[[self class] scale: self.glMatrix byY: scaleFactor];
		isIdentity = NO;
	}
}

-(void) scaleByZ: (GLfloat) scaleFactor {
	// Short-circuit an identity transform
	if ( scaleFactor != 1.0f ) {
		[[self class] scale: self.glMatrix byZ: scaleFactor];
		isIdentity = NO;
	}
}

-(void) scaleUniformlyBy: (GLfloat) scaleFactor {
	// Short-circuit an identity transform
	if ( scaleFactor != 1.0f ) {
		[[self class] scale: self.glMatrix uniformlyBy: scaleFactor];
		isIdentity = NO;
	}
}

#pragma mark Matrix transformations

+(void) transform: (GLfloat*) aGLMatrix
	  translateBy: (CC3Vector) translationVector
		 rotateBy: (CC3Vector) rotationVector
		  scaleBy: (CC3Vector) scaleVector {

	kmVec3 kmTranslation = kmVec3Make(translationVector.x, translationVector.y, translationVector.z);
	kmVec3 kmRotation = kmVec3Make(DegreesToRadians(rotationVector.x),
								   DegreesToRadians(rotationVector.y),
								   DegreesToRadians(rotationVector.z));
	kmVec3 kmScale = kmVec3Make(scaleVector.x, scaleVector.y, scaleVector.z);

	kmMat4 mXfm;
	kmMat4Transformation(&mXfm, kmTranslation, kmRotation, kmScale);
	[self multiply: aGLMatrix byMatrix: (GLfloat*)&mXfm];
}

+(void) rotateYXZ: (GLfloat*) aGLMatrix by: (CC3Vector) aVector {
	kmMat4 mRot;
	CC3Vector rotRads = CC3VectorScaleUniform(aVector, DegreesToRadiansFactor);
	kmMat4RotationYXZ(&mRot, rotRads.x, rotRads.y, rotRads.z);
	[self multiply: aGLMatrix byMatrix: (GLfloat*)&mRot];
}

+(void) rotateZYX: (GLfloat*) aGLMatrix by: (CC3Vector) aVector {
	kmMat4 mRot;
	CC3Vector rotRads = CC3VectorScaleUniform(aVector, DegreesToRadiansFactor);
	kmMat4RotationZYX(&mRot, rotRads.x, rotRads.y, rotRads.z);
	[self multiply: aGLMatrix byMatrix: (GLfloat*)&mRot];
}

+(void) rotate: (GLfloat*) aGLMatrix byX: (GLfloat) degrees {
	kmMat4 mRot;
	kmMat4RotationX(&mRot, DegreesToRadians(degrees));
	[self multiply: aGLMatrix byMatrix: (GLfloat*)&mRot];
}

+(void) rotate: (GLfloat*) aGLMatrix byY: (GLfloat) degrees {
	kmMat4 mRot;
	kmMat4RotationY(&mRot, DegreesToRadians(degrees));
	[self multiply: aGLMatrix byMatrix: (GLfloat*)&mRot];
}

+(void) rotate: (GLfloat*) aGLMatrix byZ: (GLfloat) degrees {
	kmMat4 mRot;
	kmMat4RotationZ(&mRot, DegreesToRadians(degrees));
	[self multiply: aGLMatrix byMatrix: (GLfloat*)&mRot];
}

+(void) rotate: (GLfloat*) aGLMatrix byQuaternion: (CC3Vector4) aQuaternion {
	kmMat4 mRot;
	kmMat4RotationQuaternion(&mRot, (kmQuaternion*)&aQuaternion);
	[self multiply: aGLMatrix byMatrix: (GLfloat*)&mRot];
}

+(void) translate: (GLfloat*) aGLMatrix by: (CC3Vector) aVector {
	GLfloat* m = aGLMatrix;					// Make a simple alias
	
	m[12] = aVector.x * m[0] + aVector.y * m[4] + aVector.z * m[8] + m[12];
	m[13] = aVector.x * m[1] + aVector.y * m[5] + aVector.z * m[9] + m[13];
	m[14] = aVector.x * m[2] + aVector.y * m[6] + aVector.z * m[10] + m[14];
    m[15] = aVector.x * m[3] + aVector.y * m[7] + aVector.z * m[11] + m[15];
}


+(void) translate: (GLfloat*) aGLMatrix byX: (GLfloat) distance {
	[self translate: aGLMatrix by: cc3v(distance, 0.0, 0.0)];
}

+(void) translate: (GLfloat*) aGLMatrix byY: (GLfloat) distance {
	[self translate: aGLMatrix by: cc3v(0.0, distance, 0.0)];
}

+(void) translate: (GLfloat*) aGLMatrix byZ: (GLfloat) distance {
	[self translate: aGLMatrix by: cc3v(0.0, 0.0, distance)];
}

+(void) scale: (GLfloat*) aGLMatrix by: (CC3Vector) aVector {
	GLfloat* m = aGLMatrix;					// Make a simple alias

	m[0] *= aVector.x;
	m[1] *= aVector.x;
	m[2] *= aVector.x;
	m[3] *= aVector.x;
	
	m[4] *= aVector.y;
	m[5] *= aVector.y;
	m[6] *= aVector.y;
	m[7] *= aVector.y;
	
	m[8] *= aVector.z;
	m[9] *= aVector.z;
	m[10] *= aVector.z;
	m[11] *= aVector.z;
}

+(void) scale: (GLfloat*) aGLMatrix byX: (GLfloat) scaleFactor {
	[self scale: aGLMatrix by: cc3v(scaleFactor, 1.0, 1.0)];
}

+(void) scale: (GLfloat*) aGLMatrix byY: (GLfloat) scaleFactor {
	[self scale: aGLMatrix by: cc3v(1.0, scaleFactor, 1.0)];
}

+(void) scale: (GLfloat*) aGLMatrix byZ: (GLfloat) scaleFactor {
	[self scale: aGLMatrix by: cc3v(1.0, 1.0, scaleFactor)];
}
	
+(void) scale: (GLfloat*) aGLMatrix uniformlyBy: (GLfloat) scaleFactor {
	[self scale: aGLMatrix by: cc3v(scaleFactor, scaleFactor, scaleFactor)];
}
		 

#pragma mark -
#pragma mark Instance math operations

// Includes short-circuits when one of the matrix is an identity matrix
-(void) multiplyByMatrix: (CC3GLMatrix*) aGLMatrix {

	// If other matrix is identity, this matrix doesn't change, so leave
	if (aGLMatrix.isIdentity) {
		return;
	}
	
	// If this matrix is identity, it just becomes the other matrix
	if (self.isIdentity) {
		[self populateFrom: aGLMatrix];
		return;
	}

	// Otherwise, go through with the multiplication
	[[self class] multiply: self.glMatrix byMatrix: aGLMatrix.glMatrix];
	isIdentity = NO;
}

-(CC3Vector) transformLocation: (CC3Vector) aLocation {
	// Short-circuit if this is an identity matrix
	if (isIdentity) {
		return aLocation;
	} else {
		return [[self class] transformLocation: aLocation withMatrix: self.glMatrix];
	}
}

-(CC3Vector) transformDirection: (CC3Vector) aDirection {
	// Short-circuit if this is an identity matrix
	if (isIdentity) {
		return aDirection;
	} else {
		return [[self class] transformDirection: aDirection withMatrix: self.glMatrix];
	}
}

-(CC3Vector4) transformHomogeneousVector: (CC3Vector4) aVector {
	// Short-circuit if this is an identity matrix
	if (isIdentity) {
		return aVector;
	} else {
		return [[self class] transformHomogeneousVector: aVector withMatrix: self.glMatrix];
	}
}

-(void) transpose {
	// Short-circuit if this is an identity matrix
	if (!isIdentity) {
		[[self class] transpose: self.glMatrix];
	}
}

-(BOOL) invert {
	// Short-circuit if this is an identity matrix
	if (isIdentity) {
		return YES;
	} else {
		return [[self class] invert: self.glMatrix];
	}
}

-(BOOL) invertAffine {
	// Short-circuit if this is an identity matrix
	if (isIdentity) {
		return YES;
	} else {
		return [[self class] invertAffine: self.glMatrix];
	}
}

-(void) invertRigid {
	// Short-circuit if this is an identity matrix
	if (!isIdentity) {
		[[self class] invertRigid: self.glMatrix];
	}
}


#pragma mark Matrix math operations

+(void) multiply: (GLfloat*) aGLMatrix byMatrix: (GLfloat*) anotherGLMatrix {
	kmMat4 mOut;
	kmMat4Multiply(&mOut, (kmMat4*)aGLMatrix, (kmMat4*)anotherGLMatrix);
	[self copyMatrix: (GLfloat*)&mOut into: aGLMatrix];
}

+(CC3Vector) transformLocation: (CC3Vector) aLocation withMatrix: (GLfloat*) aGLMatrix {
	return CC3VectorFromCC3Vector4([self transformHomogeneousVector: CC3Vector4FromCC3Vector(aLocation, 1.0)
														withMatrix: aGLMatrix]);
}

+(CC3Vector) transformDirection: (CC3Vector) aDirection withMatrix: (GLfloat*) aGLMatrix {
	return CC3VectorFromCC3Vector4([self transformHomogeneousVector: CC3Vector4FromCC3Vector(aDirection, 0.0)
														withMatrix: aGLMatrix]);
}

+(CC3Vector4) transformHomogeneousVector: (CC3Vector4) aVector withMatrix: (GLfloat*) aGLMatrix {
	CC3Vector4 vOut;
	GLfloat* m = aGLMatrix;					// Make a simple alias

	vOut.x = aVector.x * m[0] + aVector.y * m[4] + aVector.z * m[8] + aVector.w * m[12];
	vOut.y = aVector.x * m[1] + aVector.y * m[5] + aVector.z * m[9] + aVector.w * m[13];
	vOut.z = aVector.x * m[2] + aVector.y * m[6] + aVector.z * m[10] + aVector.w * m[14];
    vOut.w = aVector.x * m[3] + aVector.y * m[7] + aVector.z * m[11] + aVector.w * m[15];

	return vOut;
}

+(void) transpose: (GLfloat*) aGLMatrix {
	[self swap: 1 with: 4 inMatrix: aGLMatrix];
	[self swap: 2 with: 8 inMatrix: aGLMatrix];
	[self swap: 3 with: 12 inMatrix: aGLMatrix];
	[self swap: 6 with: 9 inMatrix: aGLMatrix];
	[self swap: 7 with: 13 inMatrix: aGLMatrix];
	[self swap: 11 with: 14 inMatrix: aGLMatrix];
}

+(BOOL) invert: (GLfloat*) aGLMatrix {
    kmMat4 inv;
	[[self class] copyMatrix: aGLMatrix into: (GLfloat*)&inv];

    kmMat4 tmp;
	[[self class] copyMatrix: (GLfloat*)identityContents into: (GLfloat*)&tmp];
    
	BOOL wasInverted = kmGaussJordan(&inv, &tmp);
	if (wasInverted) {
		[[self class] copyMatrix: (GLfloat*)&inv into: aGLMatrix];
	}
    return wasInverted;
}

+(BOOL) invertAffine: (GLfloat*) aGLMatrix {
/*
 M = |  L  t |
     |  0  1 |
	 
	 where L is a 3x3 linear tranformation matrix, t is a translation vector, and 0 is a row of 3 zeros
 */

	GLfloat* m = aGLMatrix;					// Make a simple alias
	BOOL wasInverted = [self invert: m];	// Invert the matrix
	m[3] = m[7] = m[11] = 0.0f;				// Ensure bottom row are exactly {0, 0, 0, 1}
	m[15] = 1.0f;
    return wasInverted;
}

+(void) invertRigid: (GLfloat*) aGLMatrix {
/*
 M = |  RT  -RT(t) |
     |  0     1    |
	 
	 where RT is the transposed 3x3 rotation matrix extracted from the 4x4 matrix
	 and t is a translation vector extracted from the 4x4 matrix
 */
	
	GLfloat* m = aGLMatrix;		// Make a simple alias
	
	// Extract translation component of matrix and remove it to leave a rotation-only matrix
	CC3Vector t = cc3v(m[12], m[13], m[14]);
	m[12] = m[13] = m[14] = 0.0f;

	// Transpose (invert) rotation matrix
	[self transpose: m];
	
	// Transform negated translation with transposed rotation matrix
	// and reinsert into transposed matrix
	t = [self transformDirection: CC3VectorNegate(t) withMatrix: m];	
	m[12] = t.x;
	m[13] = t.y;
	m[14] = t.z;
}

@end