コード例 #1
0
bool KX_RaySensor::Evaluate()
{
	bool result = false;
	bool reset = m_reset && m_level;
	m_rayHit = false; 
	m_hitObject = NULL;
	m_hitPosition[0] = 0;
	m_hitPosition[1] = 0;
	m_hitPosition[2] = 0;

	m_hitNormal[0] = 1;
	m_hitNormal[1] = 0;
	m_hitNormal[2] = 0;
	
	KX_GameObject* obj = (KX_GameObject*)GetParent();
	MT_Point3 frompoint = obj->NodeGetWorldPosition();
	MT_Matrix3x3 matje = obj->NodeGetWorldOrientation();
	MT_Matrix3x3 invmat = matje.inverse();
	
	MT_Vector3 todir;
	m_reset = false;
	switch (m_axis)
	{
	case SENS_RAY_X_AXIS: // X
		{
			todir[0] = invmat[0][0];
			todir[1] = invmat[0][1];
			todir[2] = invmat[0][2];
			break;
		}
	case SENS_RAY_Y_AXIS: // Y
		{
			todir[0] = invmat[1][0];
			todir[1] = invmat[1][1];
			todir[2] = invmat[1][2];
			break;
		}
	case SENS_RAY_Z_AXIS: // Z
		{
			todir[0] = invmat[2][0];
			todir[1] = invmat[2][1];
			todir[2] = invmat[2][2];
			break;
		}
	case SENS_RAY_NEG_X_AXIS: // -X
		{
			todir[0] = -invmat[0][0];
			todir[1] = -invmat[0][1];
			todir[2] = -invmat[0][2];
			break;
		}
	case SENS_RAY_NEG_Y_AXIS: // -Y
		{
			todir[0] = -invmat[1][0];
			todir[1] = -invmat[1][1];
			todir[2] = -invmat[1][2];
			break;
		}
	case SENS_RAY_NEG_Z_AXIS: // -Z
		{
			todir[0] = -invmat[2][0];
			todir[1] = -invmat[2][1];
			todir[2] = -invmat[2][2];
			break;
		}
	}
	todir.normalize();
	m_rayDirection[0] = todir[0];
	m_rayDirection[1] = todir[1];
	m_rayDirection[2] = todir[2];

	MT_Point3 topoint = frompoint + (m_distance) * todir;
	PHY_IPhysicsEnvironment* pe = m_scene->GetPhysicsEnvironment();

	if (!pe)
	{
		std::cout << "WARNING: Ray sensor " << GetName() << ":  There is no physics environment!" << std::endl;
		std::cout << "         Check universe for malfunction." << std::endl;
		return false;
	} 

	KX_IPhysicsController *spc = obj->GetPhysicsController();
	KX_GameObject *parent = obj->GetParent();
	if (!spc && parent)
		spc = parent->GetPhysicsController();
	
	if (parent)
		parent->Release();
	

	PHY_IPhysicsEnvironment* physics_environment = this->m_scene->GetPhysicsEnvironment();
	

	KX_RayCast::Callback<KX_RaySensor> callback(this, spc);
	KX_RayCast::RayTest(physics_environment, frompoint, topoint, callback);

	/* now pass this result to some controller */

	if (m_rayHit)
	{
		if (!m_bTriggered)
		{
			// notify logicsystem that ray is now hitting
			result = true;
			m_bTriggered = true;
		}
		else
		{
			// notify logicsystem that ray is STILL hitting ...
			result = false;

		}
	}
	else
	{
		if (m_bTriggered)
		{
			m_bTriggered = false;
			// notify logicsystem that ray JUST left the Object
			result = true;
		}
		else
		{
			result = false;
		}

	}
	if (reset)
		// force an event
		result = true;

	return result;
}
コード例 #2
0
void RAS_OpenGLRasterizer::applyTransform(double* oglmatrix,int objectdrawmode )
{
	/* FIXME:
	blender: intern/moto/include/MT_Vector3.inl:42: MT_Vector3 operator/(const
	MT_Vector3&, double): Assertion `!MT_fuzzyZero(s)' failed.

	Program received signal SIGABRT, Aborted.
	[Switching to Thread 16384 (LWP 1519)]
	0x40477571 in kill () from /lib/libc.so.6
	(gdb) bt
	#7  0x08334368 in MT_Vector3::normalized() const ()
	#8  0x0833e6ec in RAS_OpenGLRasterizer::applyTransform(RAS_IRasterizer*, double*, int) ()
	*/

	if (objectdrawmode & RAS_IPolyMaterial::BILLBOARD_SCREENALIGNED ||
		objectdrawmode & RAS_IPolyMaterial::BILLBOARD_AXISALIGNED)
	{
		// rotate the billboard/halo
		//page 360/361 3D Game Engine Design, David Eberly for a discussion
		// on screen aligned and axis aligned billboards
		// assumed is that the preprocessor transformed all billboard polygons
		// so that their normal points into the positive x direction (1.0, 0.0, 0.0)
		// when new parenting for objects is done, this rotation
		// will be moved into the object

		MT_Point3 objpos (oglmatrix[12],oglmatrix[13],oglmatrix[14]);
		MT_Point3 campos = GetCameraPosition();
		MT_Vector3 dir = (campos - objpos).safe_normalized();
		MT_Vector3 up(0,0,1.0);

		KX_GameObject* gameobj = (KX_GameObject*)m_clientobject;
		// get scaling of halo object
		MT_Vector3  size = gameobj->GetSGNode()->GetWorldScaling();

		bool screenaligned = (objectdrawmode & RAS_IPolyMaterial::BILLBOARD_SCREENALIGNED)!=0;//false; //either screen or axisaligned
		if (screenaligned)
		{
			up = (up - up.dot(dir) * dir).safe_normalized();
		} else
		{
			dir = (dir - up.dot(dir)*up).safe_normalized();
		}

		MT_Vector3 left = dir.normalized();
		dir = (up.cross(left)).normalized();

		// we have calculated the row vectors, now we keep
		// local scaling into account:

		left *= size[0];
		dir  *= size[1];
		up   *= size[2];

		double maat[16] = {left[0], left[1], left[2], 0,
		                   dir[0],  dir[1],  dir[2],  0,
		                   up[0],   up[1],   up[2],   0,
		                   0,       0,       0,       1};

		glTranslated(objpos[0],objpos[1],objpos[2]);
		glMultMatrixd(maat);

	}
	else {
		if (objectdrawmode & RAS_IPolyMaterial::SHADOW)
		{
			// shadow must be cast to the ground, physics system needed here!
			MT_Point3 frompoint(oglmatrix[12],oglmatrix[13],oglmatrix[14]);
			KX_GameObject *gameobj = (KX_GameObject*)m_clientobject;
			MT_Vector3 direction = MT_Vector3(0,0,-1);

			direction.normalize();
			direction *= 100000;

			MT_Point3 topoint = frompoint + direction;

			KX_Scene* kxscene = (KX_Scene*) m_auxilaryClientInfo;
			PHY_IPhysicsEnvironment* physics_environment = kxscene->GetPhysicsEnvironment();
			PHY_IPhysicsController* physics_controller = gameobj->GetPhysicsController();

			KX_GameObject *parent = gameobj->GetParent();
			if (!physics_controller && parent)
				physics_controller = parent->GetPhysicsController();
			if (parent)
				parent->Release();

			KX_RayCast::Callback<RAS_OpenGLRasterizer> callback(this, physics_controller, oglmatrix);
			if (!KX_RayCast::RayTest(physics_environment, frompoint, topoint, callback))
			{
				// couldn't find something to cast the shadow on...
				glMultMatrixd(oglmatrix);
			}
			else
			{ // we found the "ground", but the cast matrix doesn't take
			  // scaling in consideration, so we must apply the object scale
				MT_Vector3  size = gameobj->GetSGNode()->GetLocalScale();
				glScalef(size[0], size[1], size[2]);
			}
		} else
		{

			// 'normal' object
			glMultMatrixd(oglmatrix);
		}
	}
}
コード例 #3
0
bool KX_ConstraintActuator::Update(double curtime, bool frame)
{

	bool result = false;
	bool bNegativeEvent = IsNegativeEvent();
	RemoveAllEvents();

	if (!bNegativeEvent) {
		/* Constraint clamps the values to the specified range, with a sort of    */
		/* low-pass filtered time response, if the damp time is unequal to 0.     */

		/* Having to retrieve location/rotation and setting it afterwards may not */
		/* be efficient enough... Something to look at later.                     */
		KX_GameObject  *obj = (KX_GameObject*) GetParent();
		MT_Point3    position = obj->NodeGetWorldPosition();
		MT_Point3    newposition;
		MT_Vector3   normal, direction, refDirection;
		MT_Matrix3x3 rotation = obj->NodeGetWorldOrientation();
		MT_Scalar    filter, newdistance, cosangle;
		int axis, sign;

		if (m_posDampTime) {
			filter = m_posDampTime/(1.0+m_posDampTime);
		} else {
			filter = 0.0;
		}
		switch (m_locrot) {
		case KX_ACT_CONSTRAINT_ORIX:
		case KX_ACT_CONSTRAINT_ORIY:
		case KX_ACT_CONSTRAINT_ORIZ:
			switch (m_locrot) {
			case KX_ACT_CONSTRAINT_ORIX:
				direction[0] = rotation[0][0];
				direction[1] = rotation[1][0];
				direction[2] = rotation[2][0];
				axis = 0;
				break;
			case KX_ACT_CONSTRAINT_ORIY:
				direction[0] = rotation[0][1];
				direction[1] = rotation[1][1];
				direction[2] = rotation[2][1];
				axis = 1;
				break;
			default:
				direction[0] = rotation[0][2];
				direction[1] = rotation[1][2];
				direction[2] = rotation[2][2];
				axis = 2;
				break;
			}
			if ((m_maximumBound < (1.0f-FLT_EPSILON)) || (m_minimumBound < (1.0f-FLT_EPSILON))) {
				// reference direction needs to be evaluated
				// 1. get the cosine between current direction and target
				cosangle = direction.dot(m_refDirVector);
				if (cosangle >= (m_maximumBound-FLT_EPSILON) && cosangle <= (m_minimumBound+FLT_EPSILON)) {
					// no change to do
					result = true;
					goto CHECK_TIME;
				}
				// 2. define a new reference direction
				//    compute local axis with reference direction as X and
				//    Y in direction X refDirection plane
				MT_Vector3 zaxis = m_refDirVector.cross(direction);
				if (MT_fuzzyZero2(zaxis.length2())) {
					// direction and refDirection are identical,
					// choose any other direction to define plane
					if (direction[0] < 0.9999)
						zaxis = m_refDirVector.cross(MT_Vector3(1.0,0.0,0.0));
					else
						zaxis = m_refDirVector.cross(MT_Vector3(0.0,1.0,0.0));
				}
				MT_Vector3 yaxis = zaxis.cross(m_refDirVector);
				yaxis.normalize();
				if (cosangle > m_minimumBound) {
					// angle is too close to reference direction,
					// choose a new reference that is exactly at minimum angle
					refDirection = m_minimumBound * m_refDirVector + m_minimumSine * yaxis;
				} else {
					// angle is too large, choose new reference direction at maximum angle
					refDirection = m_maximumBound * m_refDirVector + m_maximumSine * yaxis;
				}
			} else {
				refDirection = m_refDirVector;
			}
			// apply damping on the direction
			direction = filter*direction + (1.0-filter)*refDirection;
			obj->AlignAxisToVect(direction, axis);
			result = true;
			goto CHECK_TIME;
		case KX_ACT_CONSTRAINT_DIRPX:
		case KX_ACT_CONSTRAINT_DIRPY:
		case KX_ACT_CONSTRAINT_DIRPZ:
		case KX_ACT_CONSTRAINT_DIRNX:
		case KX_ACT_CONSTRAINT_DIRNY:
		case KX_ACT_CONSTRAINT_DIRNZ:
			switch (m_locrot) {
			case KX_ACT_CONSTRAINT_DIRPX:
				normal[0] = rotation[0][0];
				normal[1] = rotation[1][0];
				normal[2] = rotation[2][0];
				axis = 0;		// axis according to KX_GameObject::AlignAxisToVect()
				sign = 0;		// X axis will be parrallel to direction of ray
				break;
			case KX_ACT_CONSTRAINT_DIRPY:
				normal[0] = rotation[0][1];
				normal[1] = rotation[1][1];
				normal[2] = rotation[2][1];
				axis = 1;
				sign = 0;
				break;
			case KX_ACT_CONSTRAINT_DIRPZ:
				normal[0] = rotation[0][2];
				normal[1] = rotation[1][2];
				normal[2] = rotation[2][2];
				axis = 2;
				sign = 0;
				break;
			case KX_ACT_CONSTRAINT_DIRNX:
				normal[0] = -rotation[0][0];
				normal[1] = -rotation[1][0];
				normal[2] = -rotation[2][0];
				axis = 0;
				sign = 1;
				break;
			case KX_ACT_CONSTRAINT_DIRNY:
				normal[0] = -rotation[0][1];
				normal[1] = -rotation[1][1];
				normal[2] = -rotation[2][1];
				axis = 1;
				sign = 1;
				break;
			case KX_ACT_CONSTRAINT_DIRNZ:
				normal[0] = -rotation[0][2];
				normal[1] = -rotation[1][2];
				normal[2] = -rotation[2][2];
				axis = 2;
				sign = 1;
				break;
			}
			normal.normalize();
			if (m_option & KX_ACT_CONSTRAINT_LOCAL) {
				// direction of the ray is along the local axis
				direction = normal;
			} else {
				switch (m_locrot) {
				case KX_ACT_CONSTRAINT_DIRPX:
					direction = MT_Vector3(1.0,0.0,0.0);
					break;
				case KX_ACT_CONSTRAINT_DIRPY:
					direction = MT_Vector3(0.0,1.0,0.0);
					break;
				case KX_ACT_CONSTRAINT_DIRPZ:
					direction = MT_Vector3(0.0,0.0,1.0);
					break;
				case KX_ACT_CONSTRAINT_DIRNX:
					direction = MT_Vector3(-1.0,0.0,0.0);
					break;
				case KX_ACT_CONSTRAINT_DIRNY:
					direction = MT_Vector3(0.0,-1.0,0.0);
					break;
				case KX_ACT_CONSTRAINT_DIRNZ:
					direction = MT_Vector3(0.0,0.0,-1.0);
					break;
				}
			}
			{
				MT_Point3 topoint = position + (m_maximumBound) * direction;
				PHY_IPhysicsEnvironment* pe = KX_GetActiveScene()->GetPhysicsEnvironment();
				PHY_IPhysicsController *spc = obj->GetPhysicsController();

				if (!pe) {
					std::cout << "WARNING: Constraint actuator " << GetName() << ":  There is no physics environment!" << std::endl;
					goto CHECK_TIME;
				}	 
				if (!spc) {
					// the object is not physical, we probably want to avoid hitting its own parent
					KX_GameObject *parent = obj->GetParent();
					if (parent) {
						spc = parent->GetPhysicsController();
						parent->Release();
					}
				}
				KX_RayCast::Callback<KX_ConstraintActuator> callback(this,dynamic_cast<PHY_IPhysicsController*>(spc));
				result = KX_RayCast::RayTest(pe, position, topoint, callback);
				if (result)	{
					MT_Vector3 newnormal = callback.m_hitNormal;
					// compute new position & orientation
					if ((m_option & (KX_ACT_CONSTRAINT_NORMAL|KX_ACT_CONSTRAINT_DISTANCE)) == 0) {
						// if none option is set, the actuator does nothing but detect ray 
						// (works like a sensor)
						goto CHECK_TIME;
					}
					if (m_option & KX_ACT_CONSTRAINT_NORMAL) {
						MT_Scalar rotFilter;
						// apply damping on the direction
						if (m_rotDampTime) {
							rotFilter = m_rotDampTime/(1.0+m_rotDampTime);
						} else {
							rotFilter = filter;
						}
						newnormal = rotFilter*normal - (1.0-rotFilter)*newnormal;
						obj->AlignAxisToVect((sign)?-newnormal:newnormal, axis);
						if (m_option & KX_ACT_CONSTRAINT_LOCAL) {
							direction = newnormal;
							direction.normalize();
						}
					}
					if (m_option & KX_ACT_CONSTRAINT_DISTANCE) {
						if (m_posDampTime) {
							newdistance = filter*(position-callback.m_hitPoint).length()+(1.0-filter)*m_minimumBound;
						} else {
							newdistance = m_minimumBound;
						}
						// logically we should cancel the speed along the ray direction as we set the
						// position along that axis
						spc = obj->GetPhysicsController();
						if (spc && spc->IsDynamic()) {
							MT_Vector3 linV = spc->GetLinearVelocity();
							// cancel the projection along the ray direction
							MT_Scalar fallspeed = linV.dot(direction);
							if (!MT_fuzzyZero(fallspeed))
								spc->SetLinearVelocity(linV-fallspeed*direction,false);
						}
					} else {
						newdistance = (position-callback.m_hitPoint).length();
					}
					newposition = callback.m_hitPoint-newdistance*direction;
				} else if (m_option & KX_ACT_CONSTRAINT_PERMANENT) {
					// no contact but still keep running
					result = true;
					goto CHECK_TIME;
				}
			}
			break; 
		case KX_ACT_CONSTRAINT_FHPX:
		case KX_ACT_CONSTRAINT_FHPY:
		case KX_ACT_CONSTRAINT_FHPZ:
		case KX_ACT_CONSTRAINT_FHNX:
		case KX_ACT_CONSTRAINT_FHNY:
		case KX_ACT_CONSTRAINT_FHNZ:
			switch (m_locrot) {
			case KX_ACT_CONSTRAINT_FHPX:
				normal[0] = -rotation[0][0];
				normal[1] = -rotation[1][0];
				normal[2] = -rotation[2][0];
				direction = MT_Vector3(1.0,0.0,0.0);
				break;
			case KX_ACT_CONSTRAINT_FHPY:
				normal[0] = -rotation[0][1];
				normal[1] = -rotation[1][1];
				normal[2] = -rotation[2][1];
				direction = MT_Vector3(0.0,1.0,0.0);
				break;
			case KX_ACT_CONSTRAINT_FHPZ:
				normal[0] = -rotation[0][2];
				normal[1] = -rotation[1][2];
				normal[2] = -rotation[2][2];
				direction = MT_Vector3(0.0,0.0,1.0);
				break;
			case KX_ACT_CONSTRAINT_FHNX:
				normal[0] = rotation[0][0];
				normal[1] = rotation[1][0];
				normal[2] = rotation[2][0];
				direction = MT_Vector3(-1.0,0.0,0.0);
				break;
			case KX_ACT_CONSTRAINT_FHNY:
				normal[0] = rotation[0][1];
				normal[1] = rotation[1][1];
				normal[2] = rotation[2][1];
				direction = MT_Vector3(0.0,-1.0,0.0);
				break;
			case KX_ACT_CONSTRAINT_FHNZ:
				normal[0] = rotation[0][2];
				normal[1] = rotation[1][2];
				normal[2] = rotation[2][2];
				direction = MT_Vector3(0.0,0.0,-1.0);
				break;
			}
			normal.normalize();
			{
				PHY_IPhysicsEnvironment* pe = KX_GetActiveScene()->GetPhysicsEnvironment();
				PHY_IPhysicsController *spc = obj->GetPhysicsController();

				if (!pe) {
					std::cout << "WARNING: Constraint actuator " << GetName() << ":  There is no physics environment!" << std::endl;
					goto CHECK_TIME;
				}	 
				if (!spc || !spc->IsDynamic()) {
					// the object is not dynamic, it won't support setting speed
					goto CHECK_TIME;
				}
				m_hitObject = NULL;
				// distance of Fh area is stored in m_minimum
				MT_Point3 topoint = position + (m_minimumBound+spc->GetRadius()) * direction;
				KX_RayCast::Callback<KX_ConstraintActuator> callback(this, spc);
				result = KX_RayCast::RayTest(pe, position, topoint, callback);
				// we expect a hit object
				if (!m_hitObject)
					result = false;
				if (result)
				{
					MT_Vector3 newnormal = callback.m_hitNormal;
					// compute new position & orientation
					MT_Scalar distance = (callback.m_hitPoint-position).length()-spc->GetRadius(); 
					// estimate the velocity of the hit point
					MT_Point3 relativeHitPoint;
					relativeHitPoint = (callback.m_hitPoint-m_hitObject->NodeGetWorldPosition());
					MT_Vector3 velocityHitPoint = m_hitObject->GetVelocity(relativeHitPoint);
					MT_Vector3 relativeVelocity = spc->GetLinearVelocity() - velocityHitPoint;
					MT_Scalar relativeVelocityRay = direction.dot(relativeVelocity);
					MT_Scalar springExtent = 1.0 - distance/m_minimumBound;
					// Fh force is stored in m_maximum
					MT_Scalar springForce = springExtent * m_maximumBound;
					// damping is stored in m_refDirection [0] = damping, [1] = rot damping
					MT_Scalar springDamp = relativeVelocityRay * m_refDirVector[0];
					MT_Vector3 newVelocity = spc->GetLinearVelocity()-(springForce+springDamp)*direction;
					if (m_option & KX_ACT_CONSTRAINT_NORMAL)
					{
						newVelocity+=(springForce+springDamp)*(newnormal-newnormal.dot(direction)*direction);
					}
					spc->SetLinearVelocity(newVelocity, false);
					if (m_option & KX_ACT_CONSTRAINT_DOROTFH)
					{
						MT_Vector3 angSpring = (normal.cross(newnormal))*m_maximumBound;
						MT_Vector3 angVelocity = spc->GetAngularVelocity();
						// remove component that is parallel to normal
						angVelocity -= angVelocity.dot(newnormal)*newnormal;
						MT_Vector3 angDamp = angVelocity * ((m_refDirVector[1]>MT_EPSILON)?m_refDirVector[1]:m_refDirVector[0]);
						spc->SetAngularVelocity(spc->GetAngularVelocity()+(angSpring-angDamp), false);
					}
				} else if (m_option & KX_ACT_CONSTRAINT_PERMANENT) {
					// no contact but still keep running
					result = true;
				}
				// don't set the position with this constraint
				goto CHECK_TIME;
			}
			break; 
		case KX_ACT_CONSTRAINT_LOCX:
		case KX_ACT_CONSTRAINT_LOCY:
		case KX_ACT_CONSTRAINT_LOCZ:
			newposition = position = obj->GetSGNode()->GetLocalPosition();
			switch (m_locrot) {
			case KX_ACT_CONSTRAINT_LOCX:
				Clamp(newposition[0], m_minimumBound, m_maximumBound);
				break;
			case KX_ACT_CONSTRAINT_LOCY:
				Clamp(newposition[1], m_minimumBound, m_maximumBound);
				break;
			case KX_ACT_CONSTRAINT_LOCZ:
				Clamp(newposition[2], m_minimumBound, m_maximumBound);
				break;
			}
			result = true;
			if (m_posDampTime) {
				newposition = filter*position + (1.0-filter)*newposition;
			}
			obj->NodeSetLocalPosition(newposition);
			goto CHECK_TIME;
		}
		if (result) {
			// set the new position but take into account parent if any
			obj->NodeSetWorldPosition(newposition);
		}
	CHECK_TIME:
		if (result && m_activeTime > 0 ) {
			if (++m_currentTime >= m_activeTime)
				result = false;
		}
	}
	if (!result) {
		m_currentTime = 0;
	}
	return result;
} /* end of KX_ConstraintActuator::Update(double curtime,double deltatime)   */