95 lines
		
	
	
		
			2.7 KiB
		
	
	
	
		
			C++
		
	
	
	
		
		
			
		
	
	
			95 lines
		
	
	
		
			2.7 KiB
		
	
	
	
		
			C++
		
	
	
	
|  | /*
 | ||
|  | * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
 | ||
|  | * | ||
|  | * This software is provided 'as-is', without any express or implied | ||
|  | * warranty.  In no event will the authors be held liable for any damages | ||
|  | * arising from the use of this software. | ||
|  | * Permission is granted to anyone to use this software for any purpose, | ||
|  | * including commercial applications, and to alter it and redistribute it | ||
|  | * freely, subject to the following restrictions: | ||
|  | * 1. The origin of this software must not be misrepresented; you must not | ||
|  | * claim that you wrote the original software. If you use this software | ||
|  | * in a product, an acknowledgment in the product documentation would be | ||
|  | * appreciated but is not required. | ||
|  | * 2. Altered source versions must be plainly marked as such, and must not be | ||
|  | * misrepresented as being the original software. | ||
|  | * 3. This notice may not be removed or altered from any source distribution. | ||
|  | */ | ||
|  | 
 | ||
|  | #include "Box2D/Common/b2Math.h"
 | ||
|  | 
 | ||
|  | const b2Vec2 b2Vec2_zero(0.0f, 0.0f); | ||
|  | 
 | ||
|  | /// Solve A * x = b, where b is a column vector. This is more efficient
 | ||
|  | /// than computing the inverse in one-shot cases.
 | ||
|  | b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const | ||
|  | { | ||
|  | 	float32 det = b2Dot(ex, b2Cross(ey, ez)); | ||
|  | 	if (det != 0.0f) | ||
|  | 	{ | ||
|  | 		det = 1.0f / det; | ||
|  | 	} | ||
|  | 	b2Vec3 x; | ||
|  | 	x.x = det * b2Dot(b, b2Cross(ey, ez)); | ||
|  | 	x.y = det * b2Dot(ex, b2Cross(b, ez)); | ||
|  | 	x.z = det * b2Dot(ex, b2Cross(ey, b)); | ||
|  | 	return x; | ||
|  | } | ||
|  | 
 | ||
|  | /// Solve A * x = b, where b is a column vector. This is more efficient
 | ||
|  | /// than computing the inverse in one-shot cases.
 | ||
|  | b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const | ||
|  | { | ||
|  | 	float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; | ||
|  | 	float32 det = a11 * a22 - a12 * a21; | ||
|  | 	if (det != 0.0f) | ||
|  | 	{ | ||
|  | 		det = 1.0f / det; | ||
|  | 	} | ||
|  | 	b2Vec2 x; | ||
|  | 	x.x = det * (a22 * b.x - a12 * b.y); | ||
|  | 	x.y = det * (a11 * b.y - a21 * b.x); | ||
|  | 	return x; | ||
|  | } | ||
|  | 
 | ||
|  | ///
 | ||
|  | void b2Mat33::GetInverse22(b2Mat33* M) const | ||
|  | { | ||
|  | 	float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; | ||
|  | 	float32 det = a * d - b * c; | ||
|  | 	if (det != 0.0f) | ||
|  | 	{ | ||
|  | 		det = 1.0f / det; | ||
|  | 	} | ||
|  | 
 | ||
|  | 	M->ex.x =  det * d;	M->ey.x = -det * b; M->ex.z = 0.0f; | ||
|  | 	M->ex.y = -det * c;	M->ey.y =  det * a; M->ey.z = 0.0f; | ||
|  | 	M->ez.x = 0.0f; M->ez.y = 0.0f; M->ez.z = 0.0f; | ||
|  | } | ||
|  | 
 | ||
|  | /// Returns the zero matrix if singular.
 | ||
|  | void b2Mat33::GetSymInverse33(b2Mat33* M) const | ||
|  | { | ||
|  | 	float32 det = b2Dot(ex, b2Cross(ey, ez)); | ||
|  | 	if (det != 0.0f) | ||
|  | 	{ | ||
|  | 		det = 1.0f / det; | ||
|  | 	} | ||
|  | 
 | ||
|  | 	float32 a11 = ex.x, a12 = ey.x, a13 = ez.x; | ||
|  | 	float32 a22 = ey.y, a23 = ez.y; | ||
|  | 	float32 a33 = ez.z; | ||
|  | 
 | ||
|  | 	M->ex.x = det * (a22 * a33 - a23 * a23); | ||
|  | 	M->ex.y = det * (a13 * a23 - a12 * a33); | ||
|  | 	M->ex.z = det * (a12 * a23 - a13 * a22); | ||
|  | 
 | ||
|  | 	M->ey.x = M->ex.y; | ||
|  | 	M->ey.y = det * (a11 * a33 - a13 * a13); | ||
|  | 	M->ey.z = det * (a13 * a12 - a11 * a23); | ||
|  | 
 | ||
|  | 	M->ez.x = M->ex.z; | ||
|  | 	M->ez.y = M->ey.z; | ||
|  | 	M->ez.z = det * (a11 * a22 - a12 * a12); | ||
|  | } |