Esempio n. 1
0
void KX_SteeringActuator::HandleActorFace(MT_Vector3& velocity)
{
	if (m_facingMode==0 && (!m_navmesh || !m_normalUp))
		return;
	KX_GameObject* curobj = (KX_GameObject*) GetParent();
	MT_Vector3 dir = m_facingMode==0 ?  curobj->NodeGetLocalOrientation().getColumn(1) : velocity;
	if (dir.fuzzyZero())
		return;
	dir.normalize();
	MT_Vector3 up(0,0,1);
	MT_Vector3 left;
	MT_Matrix3x3 mat;
	
	if (m_navmesh && m_normalUp)
	{
		dtStatNavMesh* navmesh =  m_navmesh->GetNavMesh();
		MT_Vector3 normal;
		MT_Vector3 trpos = m_navmesh->TransformToLocalCoords(curobj->NodeGetWorldPosition());
		if (getNavmeshNormal(navmesh, trpos, normal))
		{

			left = (dir.cross(up)).safe_normalized();
			dir = (-left.cross(normal)).safe_normalized();
			up = normal;
		}
	}

	switch (m_facingMode)
	{
	case 1: // TRACK X
		{
			left  = dir.safe_normalized();
			dir = -(left.cross(up)).safe_normalized();
			break;
		};
	case 2:	// TRACK Y
		{
			left  = (dir.cross(up)).safe_normalized();
			break;
		}

	case 3: // track Z
		{
			left = up.safe_normalized();
			up = dir.safe_normalized();
			dir = left;
			left  = (dir.cross(up)).safe_normalized();
			break;
		}

	case 4: // TRACK -X
		{
			left  = -dir.safe_normalized();
			dir = -(left.cross(up)).safe_normalized();
			break;
		};
	case 5: // TRACK -Y
		{
			left  = (-dir.cross(up)).safe_normalized();
			dir = -dir;
			break;
		}
	case 6: // track -Z
		{
			left = up.safe_normalized();
			up = -dir.safe_normalized();
			dir = left;
			left  = (dir.cross(up)).safe_normalized();
			break;
		}
	}

	mat.setValue (
		left[0], dir[0],up[0], 
		left[1], dir[1],up[1],
		left[2], dir[2],up[2]
	);

	
	
	KX_GameObject* parentObject = curobj->GetParent();
	if (parentObject)
	{ 
		MT_Vector3 localpos;
		localpos = curobj->GetSGNode()->GetLocalPosition();
		MT_Matrix3x3 parentmatinv;
		parentmatinv = parentObject->NodeGetWorldOrientation ().inverse ();
		mat = parentmatinv * mat;
		mat = m_parentlocalmat * mat;
		curobj->NodeSetLocalOrientation(mat);
		curobj->NodeSetLocalPosition(localpos);
	}
	else
	{
		curobj->NodeSetLocalOrientation(mat);
	}

}
void RAS_OpenGLRasterizer::FlushDebugShapes()
{
	if (m_debugShapes.empty())
		return;

	// DrawDebugLines
	GLboolean light, tex;

	light= glIsEnabled(GL_LIGHTING);
	tex= glIsEnabled(GL_TEXTURE_2D);

	if (light) glDisable(GL_LIGHTING);
	if (tex) glDisable(GL_TEXTURE_2D);

	//draw lines
	glBegin(GL_LINES);
	for (unsigned int i=0;i<m_debugShapes.size();i++)
	{
		if (m_debugShapes[i].m_type != OglDebugShape::LINE)
			continue;
		glColor4f(m_debugShapes[i].m_color[0],m_debugShapes[i].m_color[1],m_debugShapes[i].m_color[2],1.f);
		const MT_Scalar* fromPtr = &m_debugShapes[i].m_pos.x();
		const MT_Scalar* toPtr= &m_debugShapes[i].m_param.x();
		glVertex3dv(fromPtr);
		glVertex3dv(toPtr);
	}
	glEnd();

	//draw circles
	for (unsigned int i=0;i<m_debugShapes.size();i++)
	{
		if (m_debugShapes[i].m_type != OglDebugShape::CIRCLE)
			continue;
		glBegin(GL_LINE_LOOP);
		glColor4f(m_debugShapes[i].m_color[0],m_debugShapes[i].m_color[1],m_debugShapes[i].m_color[2],1.f);

		static const MT_Vector3 worldUp(0.0, 0.0, 1.0);
		MT_Vector3 norm = m_debugShapes[i].m_param;
		MT_Matrix3x3 tr;
		if (norm.fuzzyZero() || norm == worldUp)
		{
			tr.setIdentity();
		}
		else
		{
			MT_Vector3 xaxis, yaxis;
			xaxis = MT_cross(norm, worldUp);
			yaxis = MT_cross(xaxis, norm);
			tr.setValue(xaxis.x(), xaxis.y(), xaxis.z(),
				yaxis.x(), yaxis.y(), yaxis.z(),
				norm.x(), norm.y(), norm.z());
		}
		MT_Scalar rad = m_debugShapes[i].m_param2.x();
		int n = (int) m_debugShapes[i].m_param2.y();
		for (int j = 0; j<n; j++)
		{
			MT_Scalar theta = j*M_PI*2/n;
			MT_Vector3 pos(cos(theta) * rad, sin(theta) * rad, 0.0);
			pos = pos*tr;
			pos += m_debugShapes[i].m_pos;
			const MT_Scalar* posPtr = &pos.x();
			glVertex3dv(posPtr);
		}
		glEnd();
	}

	if (light) glEnable(GL_LIGHTING);
	if (tex) glEnable(GL_TEXTURE_2D);

	m_debugShapes.clear();
}
Esempio n. 3
0
bool KX_TrackToActuator::Update(double curtime, bool frame)
{
	bool result = false;	
	bool bNegativeEvent = IsNegativeEvent();
	RemoveAllEvents();

	if (bNegativeEvent)
	{
		// do nothing on negative events
	}
	else if (m_object)
	{
		KX_GameObject* curobj = (KX_GameObject*) GetParent();
		MT_Vector3 dir = ((KX_GameObject*)m_object)->NodeGetWorldPosition() - curobj->NodeGetWorldPosition();
		if (dir.length2())
			dir.normalize();
		MT_Vector3 up(0,0,1);
		
		
#ifdef DSADSA
		switch (m_upflag)
		{
		case 0:
			{
				up.setValue(1.0,0,0);
				break;
			} 
		case 1:
			{
				up.setValue(0,1.0,0);
				break;
			}
		case 2:
		default:
			{
				up.setValue(0,0,1.0);
			}
		}
#endif 
		if (m_allow3D)
		{
			up = (up - up.dot(dir) * dir).safe_normalized();
			
		}
		else
		{
			dir = (dir - up.dot(dir)*up).safe_normalized();
		}
		
		MT_Vector3 left;
		MT_Matrix3x3 mat;
		
		switch (m_trackflag)
		{
		case 0: // TRACK X
			{
				// (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up
				left  = dir.safe_normalized();
				dir = (left.cross(up)).safe_normalized();
				mat.setValue (
					left[0], dir[0],up[0], 
					left[1], dir[1],up[1],
					left[2], dir[2],up[2]
					);
				
				break;
			};
		case 1:	// TRACK Y
			{
				// (0.0 , 1.0 , 0.0 ) y direction is forward, z (0.0 , 0.0 , 1.0 ) up
				left  = (dir.cross(up)).safe_normalized();
				mat.setValue (
					left[0], dir[0],up[0], 
					left[1], dir[1],up[1],
					left[2], dir[2],up[2]
					);
				
				break;
			}
			
		case 2: // track Z
			{
				left = up.safe_normalized();
				up = dir.safe_normalized();
				dir = left;
				left  = (dir.cross(up)).safe_normalized();
				mat.setValue (
					left[0], dir[0],up[0], 
					left[1], dir[1],up[1],
					left[2], dir[2],up[2]
					);
				break;
			}
			
		case 3: // TRACK -X
			{
				// (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up
				left  = -dir.safe_normalized();
				dir = -(left.cross(up)).safe_normalized();
				mat.setValue (
					left[0], dir[0],up[0], 
					left[1], dir[1],up[1],
					left[2], dir[2],up[2]
					);
				
				break;
			};
		case 4: // TRACK -Y
			{
				// (0.0 , -1.0 , 0.0 ) -y direction is forward, z (0.0 , 0.0 , 1.0 ) up
				left  = (-dir.cross(up)).safe_normalized();
				mat.setValue (
					left[0], -dir[0],up[0], 
					left[1], -dir[1],up[1],
					left[2], -dir[2],up[2]
					);
				break;
			}
		case 5: // track -Z
			{
				left = up.safe_normalized();
				up = -dir.safe_normalized();
				dir = left;
				left  = (dir.cross(up)).safe_normalized();
				mat.setValue (
					left[0], dir[0],up[0], 
					left[1], dir[1],up[1],
					left[2], dir[2],up[2]
					);
				
				break;
			}
			
		default:
			{
				// (1.0 , 0.0 , 0.0 ) -x direction is forward, z (0.0 , 0.0 , 1.0 ) up
				left  = -dir.safe_normalized();
				dir = -(left.cross(up)).safe_normalized();
				mat.setValue (
					left[0], dir[0],up[0], 
					left[1], dir[1],up[1],
					left[2], dir[2],up[2]
					);
			}
		}
		
		MT_Matrix3x3 oldmat;
		oldmat= curobj->NodeGetWorldOrientation();
		
		/* erwin should rewrite this! */
		mat= matrix3x3_interpol(oldmat, mat, m_time);
		

		if(m_parentobj){ // check if the model is parented and calculate the child transform
				
			MT_Point3 localpos;
			localpos = curobj->GetSGNode()->GetLocalPosition();
			// Get the inverse of the parent matrix
			MT_Matrix3x3 parentmatinv;
			parentmatinv = m_parentobj->NodeGetWorldOrientation ().inverse ();				
			// transform the local coordinate system into the parents system
			mat = parentmatinv * mat;
			// append the initial parent local rotation matrix
			mat = m_parentlocalmat * mat;

			// set the models tranformation properties
			curobj->NodeSetLocalOrientation(mat);
			curobj->NodeSetLocalPosition(localpos);
			//curobj->UpdateTransform();
		}
		else
		{
			curobj->NodeSetLocalOrientation(mat);
		}

		result = true;
	}

	return result;
}