Patch #11000 approved: [new function] KX_GameObject::alignAxisToVect() Align an object's axis to a given vector

3 files changed, 104 insertions, 2 deletions

diff --git a/source/gameengine/Ketsji/KX_GameObject.cpp b/source/gameengine/Ketsji/KX_GameObject.cpp
index dada47e2fa4..c192cd01261 100644
--- a/source/gameengine/Ketsji/KX_GameObject.cpp
+++ b/source/gameengine/Ketsji/KX_GameObject.cpp
@@ -580,7 +580,68 @@ void KX_GameObject::SetObjectColor(const MT_Vector4& rgbavec)
 	m_objectColor = rgbavec;
 }
 
+void KX_GameObject::AlignAxisToVect(const MT_Vector3& dir, int axis)
+{
+	MT_Matrix3x3 orimat;
+	MT_Vector3 vect,ori,z,x,y;
+	MT_Scalar len;
 
+	vect = dir;
+	len = vect.length();
+	if (MT_fuzzyZero(len))
+	{
+		cout << "alignAxisToVect() Error: Null vector!\n";
+		return;
+	}
+	// normalize
+	vect /= len;
+	orimat = GetSGNode()->GetWorldOrientation();
+	switch (axis)
+	{	
+		case 0: //x axis
+			ori = MT_Vector3(orimat[0][2], orimat[1][2], orimat[2][2]); //pivot axis
+			if (MT_abs(vect.dot(ori)) > 1.0-3.0*MT_EPSILON) //is the vector paralell to the pivot?
+				ori = MT_Vector3(orimat[0][1], orimat[1][1], orimat[2][1]); //change the pivot!
+			x = vect; 
+			y = ori.cross(x);
+			z = x.cross(y);
+			break;
+		case 1: //y axis
+			ori = MT_Vector3(orimat[0][0], orimat[1][0], orimat[2][0]);
+			if (MT_abs(vect.dot(ori)) > 1.0-3.0*MT_EPSILON)
+				ori = MT_Vector3(orimat[0][2], orimat[1][2], orimat[2][2]);
+			y = vect;
+			z = ori.cross(y);
+			x = y.cross(z);
+			break;
+		case 2: //z axis
+			ori = MT_Vector3(orimat[0][1], orimat[1][1], orimat[2][1]);
+			if (MT_abs(vect.dot(ori)) > 1.0-3.0*MT_EPSILON)
+				ori = MT_Vector3(orimat[0][0], orimat[1][0], orimat[2][0]);
+			z = vect;
+			x = ori.cross(z);
+			y = z.cross(x);
+			break;
+		default: //wrong input?
+			cout << "alignAxisToVect(): Wrong axis '" << axis <<"'\n";
+			return;
+	}
+	x.normalize(); //normalize the vectors
+	y.normalize();
+	z.normalize();
+	orimat = MT_Matrix3x3(	x[0],y[0],z[0],
+							x[1],y[1],z[1],
+							x[2],y[2],z[2]);
+	if (GetSGNode()->GetSGParent() != NULL)
+	{
+		// the object is a child, adapt its local orientation so that 
+		// the global orientation is aligned as we want.
+		MT_Matrix3x3 invori = GetSGNode()->GetSGParent()->GetWorldOrientation().inverse();
+		NodeSetLocalOrientation(invori*orimat);
+	}
+	else
+		NodeSetLocalOrientation(orimat);
+}
 
 MT_Vector3 KX_GameObject::GetLinearVelocity(bool local)
 {
@@ -723,6 +784,7 @@ void KX_GameObject::Suspend(void)
 
 PyMethodDef KX_GameObject::Methods[] = {
 	{"setVisible",(PyCFunction) KX_GameObject::sPySetVisible, METH_VARARGS},  
+	{"alignAxisToVect",(PyCFunction) KX_GameObject::sPyAlignAxisToVect, METH_VARARGS},
 	{"setPosition", (PyCFunction) KX_GameObject::sPySetPosition, METH_VARARGS},
 	{"getPosition", (PyCFunction) KX_GameObject::sPyGetPosition, METH_VARARGS},
 	{"getOrientation", (PyCFunction) KX_GameObject::sPyGetOrientation, METH_VARARGS},
@@ -1255,7 +1317,24 @@ PyObject* KX_GameObject::PySetOrientation(PyObject* self,
 	return NULL;
 }
 
-
+PyObject* KX_GameObject::PyAlignAxisToVect(PyObject* self, 
+										  PyObject* args, 
+										  PyObject* kwds)
+{
+	PyObject* pyvect;
+	int axis = 2; //z axis is the default
+	
+	if (PyArg_ParseTuple(args,"O|i",&pyvect,&axis))
+	{
+		MT_Vector3 vect;
+		if (PyVecTo(pyvect, vect))
+		{
+			AlignAxisToVect(vect,axis);				
+			Py_Return;
+		}
+	}
+	return NULL;
+}
 
 PyObject* KX_GameObject::PySetPosition(PyObject* self, 
 									   PyObject* args, 
diff --git a/source/gameengine/Ketsji/KX_GameObject.h b/source/gameengine/Ketsji/KX_GameObject.h
index 3758651f53d..56b9f3f6375 100644
--- a/source/gameengine/Ketsji/KX_GameObject.h
+++ b/source/gameengine/Ketsji/KX_GameObject.h
@@ -259,6 +259,15 @@ public:
 	);
 
 	/** 
+	 * Align the object to a given normal.
+	 */
+		void 
+	AlignAxisToVect(
+		const MT_Vector3& vect,
+		int axis = 2 
+	);
+
+	/** 
 	 * Quick'n'dirty obcolor ipo stuff
 	 */
 
@@ -662,6 +671,7 @@ public:
 	KX_PYMETHOD(KX_GameObject,GetOrientation);
 	KX_PYMETHOD(KX_GameObject,SetOrientation);
 	KX_PYMETHOD(KX_GameObject,SetVisible);
+	KX_PYMETHOD(KX_GameObject,AlignAxisToVect);
 	KX_PYMETHOD(KX_GameObject,SuspendDynamics);
 	KX_PYMETHOD(KX_GameObject,RestoreDynamics);
 	KX_PYMETHOD(KX_GameObject,EnableRigidBody);
diff --git a/source/gameengine/PyDoc/KX_GameObject.py b/source/gameengine/PyDoc/KX_GameObject.py
index fbd896a55d1..8d29a704380 100644
--- a/source/gameengine/PyDoc/KX_GameObject.py
+++ b/source/gameengine/PyDoc/KX_GameObject.py
@@ -51,6 +51,18 @@ class KX_GameObject:
 		@type orn: 3x3 rotation matrix, or Quaternion.
 		@param orn: a rotation matrix specifying the new rotation.
 		"""
+	def alignAxisToVect(vect, axis):
+		"""
+		Aligns any of the game object's axis along the given vector.
+		
+		@type vect: 3d vector.
+		@param vect: a vector to align the axis.
+		@type axis: integer.
+		@param axis:The axis you want to align
+					- 0: X axis
+					- 1: Y axis
+					- 2: Z axis (default) 
+		"""
 	def getOrientation():
 		"""
 		Gets the game object's orientation.
@@ -213,4 +225,5 @@ class KX_GameObject:
 		@rtype: 3-tuple (L{KX_GameObject}, 3-tuple (x,y,z), 3-tuple (nx,ny,nz))
 		@return: (object,hitpoint,hitnormal) or (None,None,None)
 		"""
-	
\ No newline at end of file
+
+