Example #1
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 = curobj->NodeGetWorldPosition() - ((KX_GameObject*)m_object)->NodeGetWorldPosition();
		MT_Matrix3x3 mat;
		MT_Matrix3x3 oldmat;

		mat = vectomat(dir, m_trackflag, m_upflag, m_allow3D);
		oldmat = curobj->NodeGetWorldOrientation();
		
		/* erwin should rewrite this! */
		mat = matrix3x3_interpol(oldmat, mat, m_time);
		
		/* check if the model is parented and calculate the child transform */
		if (m_parentobj) {
				
			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;
}
Example #2
0
void KX_BlenderMaterial::setObjectMatrixData(int i, RAS_IRasterizer *ras)
{
	KX_GameObject *obj = 
		(KX_GameObject*)
		mScene->GetObjectList()->FindValue(mMaterial->mapping[i].objconame);

	if (!obj) return;

	glTexGeni(GL_S, GL_TEXTURE_GEN_MODE, GL_EYE_LINEAR );
	glTexGeni(GL_T, GL_TEXTURE_GEN_MODE, GL_EYE_LINEAR );
	glTexGeni(GL_R, GL_TEXTURE_GEN_MODE, GL_EYE_LINEAR );

	GLenum plane = GL_EYE_PLANE;

	// figure plane gen
	float proj[4] = {0.f,0.f,0.f,0.f};
	GetProjPlane(mMaterial, i, 0, proj);
	glTexGenfv(GL_S, plane, proj);
	
	GetProjPlane(mMaterial, i, 1, proj);
	glTexGenfv(GL_T, plane, proj);

	GetProjPlane(mMaterial, i, 2, proj);
	glTexGenfv(GL_R, plane, proj);

	glEnable(GL_TEXTURE_GEN_S);
	glEnable(GL_TEXTURE_GEN_T);
	glEnable(GL_TEXTURE_GEN_R);

	const MT_Matrix4x4& mvmat = ras->GetViewMatrix();

	glMatrixMode(GL_TEXTURE);
	glLoadIdentity();
	glScalef( 
		mMaterial->mapping[i].scale[0], 
		mMaterial->mapping[i].scale[1], 
		mMaterial->mapping[i].scale[2]
	);

	MT_Point3 pos = obj->NodeGetWorldPosition();
	MT_Vector4 matmul = MT_Vector4(pos[0], pos[1], pos[2], 1.f);
	MT_Vector4 t = mvmat*matmul;

	glTranslatef( (float)(-t[0]), (float)(-t[1]), (float)(-t[2]) );

	glMatrixMode(GL_MODELVIEW);

}
Example #3
0
void KX_NearSensor::SynchronizeTransform()
{
	// The near and radar sensors are using a different physical object which is 
	// not linked to the parent object, must synchronize it.
	if (m_physCtrl)
	{
		PHY_IMotionState* motionState = m_physCtrl->GetMotionState();
		KX_GameObject* parent = ((KX_GameObject*)GetParent());
		const MT_Vector3& pos = parent->NodeGetWorldPosition();
		float ori[12];
		parent->NodeGetWorldOrientation().getValue(ori);
		motionState->SetWorldPosition(pos[0], pos[1], pos[2]);
		motionState->SetWorldOrientation(ori);
		m_physCtrl->WriteMotionStateToDynamics(true);
	}
}
Example #4
0
bool KX_ObjectActuator::Update()
{
	
	bool bNegativeEvent = IsNegativeEvent();
	RemoveAllEvents();
		
	KX_GameObject *parent = static_cast<KX_GameObject *>(GetParent()); 

	if (bNegativeEvent) {
		// If we previously set the linear velocity we now have to inform
		// the physics controller that we no longer wish to apply it and that
		// it should reconcile the externally set velocity with it's 
		// own velocity.
		if (m_active_combined_velocity) {
			if (parent)
				parent->ResolveCombinedVelocities(
						m_linear_velocity,
						m_angular_velocity,
						(m_bitLocalFlag.LinearVelocity) != 0,
						(m_bitLocalFlag.AngularVelocity) != 0
					);
			m_active_combined_velocity = false;
		} 
		m_linear_damping_active = false;
		m_angular_damping_active = false;
		m_error_accumulator.setValue(0.0,0.0,0.0);
		m_previous_error.setValue(0.0,0.0,0.0);
		return false; 

	} else if (parent)
	{
		if (m_bitLocalFlag.ServoControl) 
		{
			// In this mode, we try to reach a target speed using force
			// As we don't know the friction, we must implement a generic 
			// servo control to achieve the speed in a configurable
			// v = current velocity
			// V = target velocity
			// e = V-v = speed error
			// dt = time interval since previous update
			// I = sum(e(t)*dt)
			// dv = e(t) - e(t-1)
			// KP, KD, KI : coefficient
			// F = KP*e+KI*I+KD*dv
			MT_Scalar mass = parent->GetMass();
			if (mass < MT_EPSILON)
				return false;
			MT_Vector3 v = parent->GetLinearVelocity(m_bitLocalFlag.LinearVelocity);
			if (m_reference)
			{
				const MT_Point3& mypos = parent->NodeGetWorldPosition();
				const MT_Point3& refpos = m_reference->NodeGetWorldPosition();
				MT_Point3 relpos;
				relpos = (mypos-refpos);
				MT_Vector3 vel= m_reference->GetVelocity(relpos);
				if (m_bitLocalFlag.LinearVelocity)
					// must convert in local space
					vel = parent->NodeGetWorldOrientation().transposed()*vel;
				v -= vel;
			}
			MT_Vector3 e = m_linear_velocity - v;
			MT_Vector3 dv = e - m_previous_error;
			MT_Vector3 I = m_error_accumulator + e;

			m_force = m_pid.x()*e+m_pid.y()*I+m_pid.z()*dv;
			// to automatically adapt the PID coefficient to mass;
			m_force *= mass;
			if (m_bitLocalFlag.Torque) 
			{
				if (m_force[0] > m_dloc[0])
				{
					m_force[0] = m_dloc[0];
					I[0] = m_error_accumulator[0];
				} else if (m_force[0] < m_drot[0])
				{
					m_force[0] = m_drot[0];
					I[0] = m_error_accumulator[0];
				}
			}
			if (m_bitLocalFlag.DLoc) 
			{
				if (m_force[1] > m_dloc[1])
				{
					m_force[1] = m_dloc[1];
					I[1] = m_error_accumulator[1];
				} else if (m_force[1] < m_drot[1])
				{
					m_force[1] = m_drot[1];
					I[1] = m_error_accumulator[1];
				}
			}
			if (m_bitLocalFlag.DRot) 
			{
				if (m_force[2] > m_dloc[2])
				{
					m_force[2] = m_dloc[2];
					I[2] = m_error_accumulator[2];
				} else if (m_force[2] < m_drot[2])
				{
					m_force[2] = m_drot[2];
					I[2] = m_error_accumulator[2];
				}
			}
			m_previous_error = e;
			m_error_accumulator = I;
			parent->ApplyForce(m_force,(m_bitLocalFlag.LinearVelocity) != 0);
		} else
		{
			if (!m_bitLocalFlag.ZeroForce)
			{
				parent->ApplyForce(m_force,(m_bitLocalFlag.Force) != 0);
			}
			if (!m_bitLocalFlag.ZeroTorque)
			{
				parent->ApplyTorque(m_torque,(m_bitLocalFlag.Torque) != 0);
			}
			if (!m_bitLocalFlag.ZeroDLoc)
			{
				parent->ApplyMovement(m_dloc,(m_bitLocalFlag.DLoc) != 0);
			}
			if (!m_bitLocalFlag.ZeroDRot)
			{
				parent->ApplyRotation(m_drot,(m_bitLocalFlag.DRot) != 0);
			}
			if (!m_bitLocalFlag.ZeroLinearVelocity)
			{
				if (m_bitLocalFlag.AddOrSetLinV) {
					parent->addLinearVelocity(m_linear_velocity,(m_bitLocalFlag.LinearVelocity) != 0);
				} else {
					m_active_combined_velocity = true;
					if (m_damping > 0) {
						MT_Vector3 linV;
						if (!m_linear_damping_active) {
							// delta and the start speed (depends on the existing speed in that direction)
							linV = parent->GetLinearVelocity(m_bitLocalFlag.LinearVelocity);
							// keep only the projection along the desired direction
							m_current_linear_factor = linV.dot(m_linear_velocity)/m_linear_length2;
							m_linear_damping_active = true;
						}
						if (m_current_linear_factor < 1.0)
							m_current_linear_factor += 1.0/m_damping;
						if (m_current_linear_factor > 1.0)
							m_current_linear_factor = 1.0;
						linV = m_current_linear_factor * m_linear_velocity;
						parent->setLinearVelocity(linV,(m_bitLocalFlag.LinearVelocity) != 0);
					} else {
						parent->setLinearVelocity(m_linear_velocity,(m_bitLocalFlag.LinearVelocity) != 0);
					}
				}
			}
			if (!m_bitLocalFlag.ZeroAngularVelocity)
			{
				m_active_combined_velocity = true;
				if (m_damping > 0) {
					MT_Vector3 angV;
					if (!m_angular_damping_active) {
						// delta and the start speed (depends on the existing speed in that direction)
						angV = parent->GetAngularVelocity(m_bitLocalFlag.AngularVelocity);
						// keep only the projection along the desired direction
						m_current_angular_factor = angV.dot(m_angular_velocity)/m_angular_length2;
						m_angular_damping_active = true;
					}
					if (m_current_angular_factor < 1.0)
						m_current_angular_factor += 1.0/m_damping;
					if (m_current_angular_factor > 1.0)
						m_current_angular_factor = 1.0;
					angV = m_current_angular_factor * m_angular_velocity;
					parent->setAngularVelocity(angV,(m_bitLocalFlag.AngularVelocity) != 0);
				} else {
					parent->setAngularVelocity(m_angular_velocity,(m_bitLocalFlag.AngularVelocity) != 0);
				}
			}
		}
		
	}
	return true;
}
Example #5
0
bool KX_ObjectActuator::Update()
{
	
	bool bNegativeEvent = IsNegativeEvent();
	RemoveAllEvents();
		
	KX_GameObject *parent = static_cast<KX_GameObject *>(GetParent()); 
	PHY_ICharacter *character = parent->GetScene()->GetPhysicsEnvironment()->GetCharacterController(parent);

	if (bNegativeEvent) {
		// If we previously set the linear velocity we now have to inform
		// the physics controller that we no longer wish to apply it and that
		// it should reconcile the externally set velocity with it's 
		// own velocity.
		if (m_active_combined_velocity) {
			if (parent)
				parent->ResolveCombinedVelocities(
						m_linear_velocity,
						m_angular_velocity,
						(m_bitLocalFlag.LinearVelocity) != 0,
						(m_bitLocalFlag.AngularVelocity) != 0
					);
			m_active_combined_velocity = false;
		}

		// Explicitly stop the movement if we're using character motion
		if (m_bitLocalFlag.CharacterMotion) {
			character->SetWalkDirection(MT_Vector3 (0.0f, 0.0f, 0.0f));
		}

		m_linear_damping_active = false;
		m_angular_damping_active = false;
		m_error_accumulator.setValue(0.0f,0.0f,0.0f);
		m_previous_error.setValue(0.0f,0.0f,0.0f);
		m_jumping = false;
		return false; 

	} else if (parent)
	{
		if (m_bitLocalFlag.ServoControl) 
		{
			// In this mode, we try to reach a target speed using force
			// As we don't know the friction, we must implement a generic 
			// servo control to achieve the speed in a configurable
			// v = current velocity
			// V = target velocity
			// e = V-v = speed error
			// dt = time interval since previous update
			// I = sum(e(t)*dt)
			// dv = e(t) - e(t-1)
			// KP, KD, KI : coefficient
			// F = KP*e+KI*I+KD*dv
			MT_Scalar mass = parent->GetMass();
			if (mass < MT_EPSILON)
				return false;
			MT_Vector3 v = parent->GetLinearVelocity(m_bitLocalFlag.LinearVelocity);
			if (m_reference)
			{
				const MT_Point3& mypos = parent->NodeGetWorldPosition();
				const MT_Point3& refpos = m_reference->NodeGetWorldPosition();
				MT_Point3 relpos;
				relpos = (mypos-refpos);
				MT_Vector3 vel= m_reference->GetVelocity(relpos);
				if (m_bitLocalFlag.LinearVelocity)
					// must convert in local space
					vel = parent->NodeGetWorldOrientation().transposed()*vel;
				v -= vel;
			}
			MT_Vector3 e = m_linear_velocity - v;
			MT_Vector3 dv = e - m_previous_error;
			MT_Vector3 I = m_error_accumulator + e;

			m_force = m_pid.x()*e+m_pid.y()*I+m_pid.z()*dv;
			// to automatically adapt the PID coefficient to mass;
			m_force *= mass;
			if (m_bitLocalFlag.Torque) 
			{
				if (m_force[0] > m_dloc[0])
				{
					m_force[0] = m_dloc[0];
					I[0] = m_error_accumulator[0];
				} else if (m_force[0] < m_drot[0])
				{
					m_force[0] = m_drot[0];
					I[0] = m_error_accumulator[0];
				}
			}
			if (m_bitLocalFlag.DLoc) 
			{
				if (m_force[1] > m_dloc[1])
				{
					m_force[1] = m_dloc[1];
					I[1] = m_error_accumulator[1];
				} else if (m_force[1] < m_drot[1])
				{
					m_force[1] = m_drot[1];
					I[1] = m_error_accumulator[1];
				}
			}
			if (m_bitLocalFlag.DRot) 
			{
				if (m_force[2] > m_dloc[2])
				{
					m_force[2] = m_dloc[2];
					I[2] = m_error_accumulator[2];
				} else if (m_force[2] < m_drot[2])
				{
					m_force[2] = m_drot[2];
					I[2] = m_error_accumulator[2];
				}
			}
			m_previous_error = e;
			m_error_accumulator = I;
			parent->ApplyForce(m_force,(m_bitLocalFlag.LinearVelocity) != 0);
		}
		else if (m_bitLocalFlag.CharacterMotion) {
			MT_Vector3 dir = m_dloc;

			if (m_bitLocalFlag.DLoc) {
				MT_Matrix3x3 basis = parent->GetPhysicsController()->GetOrientation();
				dir = basis * dir;
			}

			if (m_bitLocalFlag.AddOrSetCharLoc) {
				MT_Vector3 old_dir = character->GetWalkDirection();

				if (!old_dir.fuzzyZero()) {
					MT_Scalar mag = old_dir.length();

					dir = dir + old_dir;
					if (!dir.fuzzyZero())
						dir = dir.normalized() * mag;
				}
			}

			// We always want to set the walk direction since a walk direction of (0, 0, 0) should stop the character
			character->SetWalkDirection(dir/parent->GetScene()->GetPhysicsEnvironment()->GetNumTimeSubSteps());

			if (!m_bitLocalFlag.ZeroDRot)
			{
				parent->ApplyRotation(m_drot,(m_bitLocalFlag.DRot) != 0);
			}

			if (m_bitLocalFlag.CharacterJump) {
				if (!m_jumping) {
					character->Jump();
					m_jumping = true;
				}
				else if (character->OnGround())
					m_jumping = false;
			}
		}
		else {
			if (!m_bitLocalFlag.ZeroForce)
			{
				parent->ApplyForce(m_force,(m_bitLocalFlag.Force) != 0);
			}
			if (!m_bitLocalFlag.ZeroTorque)
			{
				parent->ApplyTorque(m_torque,(m_bitLocalFlag.Torque) != 0);
			}
			if (!m_bitLocalFlag.ZeroDLoc)
			{
				parent->ApplyMovement(m_dloc,(m_bitLocalFlag.DLoc) != 0);
			}
			if (!m_bitLocalFlag.ZeroDRot)
			{
				parent->ApplyRotation(m_drot,(m_bitLocalFlag.DRot) != 0);
			}

			if (m_bitLocalFlag.ZeroLinearVelocity) {
				if (!m_bitLocalFlag.AddOrSetLinV) {
					/* No need to select local or world, as the velocity is zero anyway,
					 * and setLinearVelocity() converts local to world first. We do need to
					 * pass a true zero vector, as m_linear_velocity is only fuzzily zero. */
					parent->setLinearVelocity(MT_Vector3(0, 0, 0), false);
				}
			}
			else {
				if (m_bitLocalFlag.AddOrSetLinV) {
					parent->addLinearVelocity(m_linear_velocity,(m_bitLocalFlag.LinearVelocity) != 0);
				} else {
					m_active_combined_velocity = true;
					if (m_damping > 0) {
						MT_Vector3 linV;
						if (!m_linear_damping_active) {
							// delta and the start speed (depends on the existing speed in that direction)
							linV = parent->GetLinearVelocity(m_bitLocalFlag.LinearVelocity);
							// keep only the projection along the desired direction
							m_current_linear_factor = linV.dot(m_linear_velocity)/m_linear_length2;
							m_linear_damping_active = true;
						}
						if (m_current_linear_factor < 1.0f)
							m_current_linear_factor += 1.0f/m_damping;
						if (m_current_linear_factor > 1.0f)
							m_current_linear_factor = 1.0f;
						linV = m_current_linear_factor * m_linear_velocity;
						parent->setLinearVelocity(linV,(m_bitLocalFlag.LinearVelocity) != 0);
					} else {
						parent->setLinearVelocity(m_linear_velocity,(m_bitLocalFlag.LinearVelocity) != 0);
					}
				}
			}
			if (m_bitLocalFlag.ZeroAngularVelocity) {
				/* No need to select local or world, as the velocity is zero anyway,
				 * and setAngularVelocity() converts local to world first. We do need to
				 * pass a true zero vector, as m_angular_velocity is only fuzzily zero. */
				parent->setAngularVelocity(MT_Vector3(0, 0, 0), false);
			}
			else {
				m_active_combined_velocity = true;
				if (m_damping > 0) {
					MT_Vector3 angV;
					if (!m_angular_damping_active) {
						// delta and the start speed (depends on the existing speed in that direction)
						angV = parent->GetAngularVelocity(m_bitLocalFlag.AngularVelocity);
						// keep only the projection along the desired direction
						m_current_angular_factor = angV.dot(m_angular_velocity)/m_angular_length2;
						m_angular_damping_active = true;
					}
					if (m_current_angular_factor < 1.0f)
						m_current_angular_factor += 1.0f/m_damping;
					if (m_current_angular_factor > 1.0f)
						m_current_angular_factor = 1.0f;
					angV = m_current_angular_factor * m_angular_velocity;
					parent->setAngularVelocity(angV,(m_bitLocalFlag.AngularVelocity) != 0);
				} else {
					parent->setAngularVelocity(m_angular_velocity,(m_bitLocalFlag.AngularVelocity) != 0);
				}
			}
		}
		
	}
	return true;
}
Example #6
0
bool KX_SoundActuator::Update(double curtime, bool frame)
{
	if (!frame)
		return true;
	bool result = false;

#ifdef WITH_AUDASPACE
	// do nothing on negative events, otherwise sounds are played twice!
	bool bNegativeEvent = IsNegativeEvent();
	bool bPositiveEvent = m_posevent;
#endif  // WITH_AUDASPACE
	
	RemoveAllEvents();

#ifdef WITH_AUDASPACE
	if (!m_sound)
		return false;

	// actual audio device playing state
	bool isplaying = m_handle ? (AUD_Handle_getStatus(m_handle) == AUD_STATUS_PLAYING) : false;

	if (bNegativeEvent)
	{
		// here must be a check if it is still playing
		if (m_isplaying && isplaying)
		{
			switch (m_type)
			{
			case KX_SOUNDACT_PLAYSTOP:
			case KX_SOUNDACT_LOOPSTOP:
			case KX_SOUNDACT_LOOPBIDIRECTIONAL_STOP:
				{
					// stop immediately
					if (m_handle)
					{
						AUD_Handle_stop(m_handle);
						m_handle = NULL;
					}
					break;
				}
			case KX_SOUNDACT_PLAYEND:
				{
					// do nothing, sound will stop anyway when it's finished
					break;
				}
			case KX_SOUNDACT_LOOPEND:
			case KX_SOUNDACT_LOOPBIDIRECTIONAL:
				{
					// stop the looping so that the sound stops when it finished
					if (m_handle)
						AUD_Handle_setLoopCount(m_handle, 0);
					break;
				}
			default:
				// implement me !!
				break;
			}
		}
		// remember that we tried to stop the actuator
		m_isplaying = false;
	}
	
#if 1
	// Warning: when de-activating the actuator, after a single negative event this runs again with...
	// m_posevent==false && m_posevent==false, in this case IsNegativeEvent() returns false 
	// and assumes this is a positive event.
	// check that we actually have a positive event so as not to play sounds when being disabled.
	else if (bPositiveEvent)  /* <- added since 2.49 */
#else
	else	// <- works in most cases except a loop-end sound will never stop unless
			// the negative pulse is done continuesly
#endif
	{
		if (!m_isplaying)
			play();
	}
	// verify that the sound is still playing
	isplaying = m_handle ? (AUD_Handle_getStatus(m_handle) == AUD_STATUS_PLAYING) : false;

	if (isplaying)
	{
		if (m_is3d)
		{
			KX_Camera* cam = KX_GetActiveScene()->GetActiveCamera();
			if (cam)
			{
				KX_GameObject* obj = (KX_GameObject*)this->GetParent();
				MT_Vector3 p;
				MT_Matrix3x3 Mo;
				float data[4];

				Mo = cam->NodeGetWorldOrientation().inverse();
				p = (obj->NodeGetWorldPosition() - cam->NodeGetWorldPosition());
				p = Mo * p;
				p.getValue(data);
				AUD_Handle_setLocation(m_handle, data);
				p = (obj->GetLinearVelocity() - cam->GetLinearVelocity());
				p = Mo * p;
				p.getValue(data);
				AUD_Handle_setVelocity(m_handle, data);
				(Mo * obj->NodeGetWorldOrientation()).getRotation().getValue(data);
				AUD_Handle_setOrientation(m_handle, data);
			}
		}
		result = true;
	}
	else
	{
		m_isplaying = false;
		result = false;
	}
#endif  // WITH_AUDASPACE

	return result;
}
Example #7
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_Vector3    position = obj->NodeGetWorldPosition();
		MT_Vector3    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.0f+m_posDampTime);
		} else {
			filter = 0.0f;
		}
		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.9999f)
						zaxis = m_refDirVector.cross(MT_Vector3(1.0f,0.0f,0.0f));
					else
						zaxis = m_refDirVector.cross(MT_Vector3(0.0f,1.0f,0.0f));
				}
				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.0f-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.0f,0.0f,0.0f);
					break;
				case KX_ACT_CONSTRAINT_DIRPY:
					direction = MT_Vector3(0.0f,1.0f,0.0f);
					break;
				case KX_ACT_CONSTRAINT_DIRPZ:
					direction = MT_Vector3(0.0f,0.0f,1.0f);
					break;
				case KX_ACT_CONSTRAINT_DIRNX:
					direction = MT_Vector3(-1.0f,0.0f,0.0f);
					break;
				case KX_ACT_CONSTRAINT_DIRNY:
					direction = MT_Vector3(0.0f,-1.0f,0.0f);
					break;
				case KX_ACT_CONSTRAINT_DIRNZ:
					direction = MT_Vector3(0.0f,0.0f,-1.0f);
					break;
				}
			}
			{
				MT_Vector3 topoint = position + (m_maximumBound) * direction;
				PHY_IPhysicsEnvironment* pe = KX_GetActiveScene()->GetPhysicsEnvironment();
				PHY_IPhysicsController *spc = obj->GetPhysicsController();

				if (!pe) {
					CM_LogicBrickWarning(this, "there is no physics environment!");
					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();
					}
				}
				KX_RayCast::Callback<KX_ConstraintActuator, void> 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.0f+m_rotDampTime);
						} else {
							rotFilter = filter;
						}
						newnormal = rotFilter*normal - (1.0f-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.0f-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.0f,0.0f,0.0f);
				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.0f,1.0f,0.0f);
				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.0f,0.0f,1.0f);
				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.0f,0.0f,0.0f);
				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.0f,-1.0f,0.0f);
				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.0f,0.0f,-1.0f);
				break;
			}
			normal.normalize();
			{
				PHY_IPhysicsEnvironment* pe = KX_GetActiveScene()->GetPhysicsEnvironment();
				PHY_IPhysicsController *spc = obj->GetPhysicsController();

				if (!pe) {
					CM_LogicBrickWarning(this, "there is no physics environment!");
					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_Vector3 topoint = position + (m_minimumBound+spc->GetRadius()) * direction;
				KX_RayCast::Callback<KX_ConstraintActuator, void> 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_Vector3 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.0f - 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.0f-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)   */
Example #8
0
	///this generates ipo curves for position, rotation, allowing to use game physics in animation
void	KX_BlenderSceneConverter::WritePhysicsObjectToAnimationIpo(int frameNumber)
{

	KX_SceneList* scenes = m_ketsjiEngine->CurrentScenes();
	int numScenes = scenes->size();
	int i;
	for (i=0;i<numScenes;i++)
	{
		KX_Scene* scene = scenes->at(i);
		//PHY_IPhysicsEnvironment* physEnv = scene->GetPhysicsEnvironment();
		CListValue* parentList = scene->GetObjectList();
		int numObjects = parentList->GetCount();
		int g;
		for (g=0;g<numObjects;g++)
		{
			KX_GameObject* gameObj = (KX_GameObject*)parentList->GetValue(g);
			Object* blenderObject = gameObj->GetBlenderObject();
			if (blenderObject && blenderObject->parent==NULL && gameObj->IsDynamic())
			{
				//KX_IPhysicsController* physCtrl = gameObj->GetPhysicsController();

				if (blenderObject->adt==NULL)
					BKE_id_add_animdata(&blenderObject->id);

				if (blenderObject->adt)
				{
					const MT_Point3& position = gameObj->NodeGetWorldPosition();
					//const MT_Vector3& scale = gameObj->NodeGetWorldScaling();
					const MT_Matrix3x3& orn = gameObj->NodeGetWorldOrientation();

					position.getValue(blenderObject->loc);

					float tmat[3][3];
					for (int r=0;r<3;r++)
						for (int c=0;c<3;c++)
							tmat[r][c] = (float)orn[c][r];

					mat3_to_compatible_eul(blenderObject->rot, blenderObject->rot, tmat);

					insert_keyframe(NULL, &blenderObject->id, NULL, NULL, "location", -1, (float)frameNumber, INSERTKEY_FAST);
					insert_keyframe(NULL, &blenderObject->id, NULL, NULL, "rotation_euler", -1, (float)frameNumber, INSERTKEY_FAST);

#if 0
					const MT_Point3& position = gameObj->NodeGetWorldPosition();
					//const MT_Vector3& scale = gameObj->NodeGetWorldScaling();
					const MT_Matrix3x3& orn = gameObj->NodeGetWorldOrientation();
					
					float eulerAngles[3];	
					float eulerAnglesOld[3] = {0.0f, 0.0f, 0.0f};						
					float tmat[3][3];
					
					// XXX animato
					Ipo* ipo = blenderObject->ipo;

					//create the curves, if not existing, set linear if new

					IpoCurve *icu_lx = findIpoCurve((IpoCurve *)ipo->curve.first,"LocX");
					if (!icu_lx) {
						icu_lx = verify_ipocurve(&blenderObject->id, ipo->blocktype, NULL, NULL, NULL, OB_LOC_X, 1);
						if (icu_lx) icu_lx->ipo = IPO_LIN;
					}
					IpoCurve *icu_ly = findIpoCurve((IpoCurve *)ipo->curve.first,"LocY");
					if (!icu_ly) {
						icu_ly = verify_ipocurve(&blenderObject->id, ipo->blocktype, NULL, NULL, NULL, OB_LOC_Y, 1);
						if (icu_ly) icu_ly->ipo = IPO_LIN;
					}
					IpoCurve *icu_lz = findIpoCurve((IpoCurve *)ipo->curve.first,"LocZ");
					if (!icu_lz) {
						icu_lz = verify_ipocurve(&blenderObject->id, ipo->blocktype, NULL, NULL, NULL, OB_LOC_Z, 1);
						if (icu_lz) icu_lz->ipo = IPO_LIN;
					}
					IpoCurve *icu_rx = findIpoCurve((IpoCurve *)ipo->curve.first,"RotX");
					if (!icu_rx) {
						icu_rx = verify_ipocurve(&blenderObject->id, ipo->blocktype, NULL, NULL, NULL, OB_ROT_X, 1);
						if (icu_rx) icu_rx->ipo = IPO_LIN;
					}
					IpoCurve *icu_ry = findIpoCurve((IpoCurve *)ipo->curve.first,"RotY");
					if (!icu_ry) {
						icu_ry = verify_ipocurve(&blenderObject->id, ipo->blocktype, NULL, NULL, NULL, OB_ROT_Y, 1);
						if (icu_ry) icu_ry->ipo = IPO_LIN;
					}
					IpoCurve *icu_rz = findIpoCurve((IpoCurve *)ipo->curve.first,"RotZ");
					if (!icu_rz) {
						icu_rz = verify_ipocurve(&blenderObject->id, ipo->blocktype, NULL, NULL, NULL, OB_ROT_Z, 1);
						if (icu_rz) icu_rz->ipo = IPO_LIN;
					}
					
					if (icu_rx) eulerAnglesOld[0]= eval_icu( icu_rx, frameNumber - 1 ) / ((180 / 3.14159265f) / 10);
					if (icu_ry) eulerAnglesOld[1]= eval_icu( icu_ry, frameNumber - 1 ) / ((180 / 3.14159265f) / 10);
					if (icu_rz) eulerAnglesOld[2]= eval_icu( icu_rz, frameNumber - 1 ) / ((180 / 3.14159265f) / 10);
					
					// orn.getValue((float *)tmat); // uses the wrong ordering, cant use this
					for (int r=0;r<3;r++)
						for (int c=0;c<3;c++)
							tmat[r][c] = orn[c][r];
					
					// mat3_to_eul( eulerAngles,tmat); // better to use Mat3ToCompatibleEul
					mat3_to_compatible_eul( eulerAngles, eulerAnglesOld,tmat);
					
					//eval_icu
					for (int x = 0; x < 3; x++)
						eulerAngles[x] *= (float) ((180 / 3.14159265f) / 10.0);
					
					//fill the curves with data
					if (icu_lx) insert_vert_icu(icu_lx, frameNumber, position.x(), 1);
					if (icu_ly) insert_vert_icu(icu_ly, frameNumber, position.y(), 1);
					if (icu_lz) insert_vert_icu(icu_lz, frameNumber, position.z(), 1);
					if (icu_rx) insert_vert_icu(icu_rx, frameNumber, eulerAngles[0], 1);
					if (icu_ry) insert_vert_icu(icu_ry, frameNumber, eulerAngles[1], 1);
					if (icu_rz) insert_vert_icu(icu_rz, frameNumber, eulerAngles[2], 1);
					
					// Handles are corrected at the end, testhandles_ipocurve isn't needed yet
#endif
				}
			}
		}
	}
}
Example #9
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;
}
Example #10
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);
	}

}
Example #11
0
bool KX_SteeringActuator::Update(double curtime, bool frame)
{
	if (frame)
	{
		double delta =  curtime - m_updateTime;
		m_updateTime = curtime;
		
		if (m_posevent && !m_isActive)
		{
			delta = 0.0;
			m_pathUpdateTime = -1.0;
			m_updateTime = curtime;
			m_isActive = true;
		}
		bool bNegativeEvent = IsNegativeEvent();
		if (bNegativeEvent)
			m_isActive = false;

		RemoveAllEvents();

		if (!delta)
			return true;

		if (bNegativeEvent || !m_target)
			return false; // do nothing on negative events

		KX_GameObject *obj = (KX_GameObject*) GetParent();
		const MT_Vector3& mypos = obj->NodeGetWorldPosition();
		const MT_Vector3& targpos = m_target->NodeGetWorldPosition();
		MT_Vector3 vectotarg = targpos - mypos;
		MT_Vector3 vectotarg2d = vectotarg;
		vectotarg2d.z() = 0.0f;
		m_steerVec = MT_Vector3(0.0f, 0.0f, 0.0f);
		bool apply_steerforce = false;
		bool terminate = true;

		switch (m_mode) {
			case KX_STEERING_SEEK:
				if (vectotarg2d.length2()>m_distance*m_distance)
				{
					terminate = false;
					m_steerVec = vectotarg;
					m_steerVec.normalize();
					apply_steerforce = true;
				}
				break;
			case KX_STEERING_FLEE:
				if (vectotarg2d.length2()<m_distance*m_distance)
				{
					terminate = false;
					m_steerVec = -vectotarg;
					m_steerVec.normalize();
					apply_steerforce = true;
				}
				break;
			case KX_STEERING_PATHFOLLOWING:
				if (m_navmesh && vectotarg.length2()>m_distance*m_distance)
				{
					terminate = false;

					static const MT_Scalar WAYPOINT_RADIUS(0.25f);

					if (m_pathUpdateTime<0 || (m_pathUpdatePeriod>=0 && 
												curtime - m_pathUpdateTime>((double)m_pathUpdatePeriod/1000.0)))
					{
						m_pathUpdateTime = curtime;
						m_pathLen = m_navmesh->FindPath(mypos, targpos, m_path, MAX_PATH_LENGTH);
						m_wayPointIdx = m_pathLen > 1 ? 1 : -1;
					}

					if (m_wayPointIdx>0)
					{
						MT_Vector3 waypoint(&m_path[3*m_wayPointIdx]);
						if ((waypoint-mypos).length2()<WAYPOINT_RADIUS*WAYPOINT_RADIUS)
						{
							m_wayPointIdx++;
							if (m_wayPointIdx>=m_pathLen)
							{
								m_wayPointIdx = -1;
								terminate = true;
							}
							else
								waypoint.setValue(&m_path[3*m_wayPointIdx]);
						}

						m_steerVec = waypoint - mypos;
						apply_steerforce = true;

						
						if (m_enableVisualization)
						{
							//debug draw
							static const MT_Vector4 PATH_COLOR(1.0f, 0.0f, 0.0f, 1.0f);
							m_navmesh->DrawPath(m_path, m_pathLen, PATH_COLOR);
						}
					}
					
				}
				break;
		}

		if (apply_steerforce)
		{
			bool isdyna = obj->IsDynamic();
			if (isdyna)
				m_steerVec.z() = 0;
			if (!m_steerVec.fuzzyZero())
				m_steerVec.normalize();
			MT_Vector3 newvel = m_velocity * m_steerVec;

			//adjust velocity to avoid obstacles
			if (m_simulation && m_obstacle /*&& !newvel.fuzzyZero()*/)
			{
				if (m_enableVisualization)
					KX_RasterizerDrawDebugLine(mypos, mypos + newvel, MT_Vector4(1.0f, 0.0f, 0.0f, 1.0f));
				m_simulation->AdjustObstacleVelocity(m_obstacle, m_mode!=KX_STEERING_PATHFOLLOWING ? m_navmesh : NULL,
								newvel, m_acceleration*(float)delta, m_turnspeed/(180.0f*(float)(M_PI*delta)));
				if (m_enableVisualization)
					KX_RasterizerDrawDebugLine(mypos, mypos + newvel, MT_Vector4(0.0f, 1.0f, 0.0f, 1.0f));
			}

			HandleActorFace(newvel);
			if (isdyna)
			{
				//temporary solution: set 2D steering velocity directly to obj
				//correct way is to apply physical force
				MT_Vector3 curvel = obj->GetLinearVelocity();

				if (m_lockzvel)
					newvel.z() = 0.0f;
				else
					newvel.z() = curvel.z();

				obj->setLinearVelocity(newvel, false);
			}
			else
			{
				MT_Vector3 movement = delta*newvel;
				obj->ApplyMovement(movement, false);
			}
		}
		else
		{
			if (m_simulation && m_obstacle)
			{
				m_obstacle->dvel[0] = 0.f;
				m_obstacle->dvel[1] = 0.f;
			}
			
		}

		if (terminate && m_isSelfTerminated)
			return false;
	}

	return true;
}
Example #12
0
bool KX_CameraActuator::Update(double curtime, bool frame)
{
	/* wondering... is it really necessary/desirable to suppress negative    */
	/* events here?                                                          */
	bool bNegativeEvent = IsNegativeEvent();
	RemoveAllEvents();

	if (bNegativeEvent || !m_ob) 
		return false;
	
	KX_GameObject *obj = (KX_GameObject*) GetParent();
	MT_Point3 from = obj->NodeGetWorldPosition();
	MT_Matrix3x3 frommat = obj->NodeGetWorldOrientation();
	/* These casts are _very_ dangerous!!! */
	MT_Point3 lookat = ((KX_GameObject*)m_ob)->NodeGetWorldPosition();
	MT_Matrix3x3 actormat = ((KX_GameObject*)m_ob)->NodeGetWorldOrientation();

	float fp1[3]={0}, fp2[3]={0}, rc[3];
	float inp, fac; //, factor = 0.0; /* some factor...                                    */
	float mindistsq, maxdistsq, distsq;
	float mat[3][3];
	
	/* The rules:                                                            */
	/* CONSTRAINT 1: not implemented */
	/* CONSTRAINT 2: can camera see actor?              */
	/* CONSTRAINT 3: fixed height relative to floor below actor.             */
	/* CONSTRAINT 4: camera rotates behind actor                              */
	/* CONSTRAINT 5: minimum / maximum distance                             */
	/* CONSTRAINT 6: again: fixed height relative to floor below actor        */
	/* CONSTRAINT 7: track to floor below actor                               */
	/* CONSTRAINT 8: look a little bit left or right, depending on how the
	 *
	 * character is looking (horizontal x)
	 */

	/* ...and then set the camera position. Since we assume the parent of    */
	/* this actuator is always a camera, just set the parent position and    */
	/* rotation. We do not check whether we really have a camera as parent.  */
	/* It may be better to turn this into a general tracking actuator later  */
	/* on, since lots of plausible relations can be filled in here.          */

	/* ... set up some parameters ...                                        */
	/* missing here: the 'floorloc' of the actor's shadow */

	mindistsq= m_minHeight*m_minHeight;
	maxdistsq= m_maxHeight*m_maxHeight;

	/* C1: not checked... is a future option                                 */

	/* C2: blender test_visibility function. Can this be a ray-test?         */

	/* C3: fixed height  */
	from[2] = (15.0f * from[2] + lookat[2] + m_height) / 16.0f;


	/* C4: camera behind actor   */
	switch (m_axis) {
		case OB_POSX:
			/* X */
			fp1[0] = actormat[0][0];
			fp1[1] = actormat[1][0];
			fp1[2] = actormat[2][0];

			fp2[0] = frommat[0][0];
			fp2[1] = frommat[1][0];
			fp2[2] = frommat[2][0];
			break;
		case OB_POSY:
			/* Y */
			fp1[0] = actormat[0][1];
			fp1[1] = actormat[1][1];
			fp1[2] = actormat[2][1];

			fp2[0] = frommat[0][1];
			fp2[1] = frommat[1][1];
			fp2[2] = frommat[2][1];
			break;
		case OB_NEGX:
			/* -X */
			fp1[0] = -actormat[0][0];
			fp1[1] = -actormat[1][0];
			fp1[2] = -actormat[2][0];

			fp2[0] = frommat[0][0];
			fp2[1] = frommat[1][0];
			fp2[2] = frommat[2][0];
			break;
		case OB_NEGY:
			/* -Y */
			fp1[0] = -actormat[0][1];
			fp1[1] = -actormat[1][1];
			fp1[2] = -actormat[2][1];

			fp2[0] = frommat[0][1];
			fp2[1] = frommat[1][1];
			fp2[2] = frommat[2][1];
			break;
		default:
			assert(0);
			break;
	}

	inp = fp1[0]*fp2[0] + fp1[1]*fp2[1] + fp1[2]*fp2[2];
	fac = (-1.0f + inp) * m_damping;

	from[0] += fac * fp1[0];
	from[1] += fac * fp1[1];
	from[2] += fac * fp1[2];
	
	/* only for it lies: cross test and perpendicular bites up */
	if (inp < 0.0f) {
		/* Don't do anything if the cross product is too small.
		 * The camera up-axis becomes unstable and starts to oscillate.
		 * The 0.01f threshold is arbitrary but seems to work well in practice. */
		float cross = fp1[0] * fp2[1] - fp1[1] * fp2[0];
		if (cross > 0.01f) {
			from[0] -= fac * fp1[1];
			from[1] += fac * fp1[0];
		}
		else if (cross < -0.01f) {
			from[0] += fac * fp1[1];
			from[1] -= fac * fp1[0];
		}
	}

	/* CONSTRAINT 5: minimum / maximum distance */

	rc[0] = (lookat[0]-from[0]);
	rc[1] = (lookat[1]-from[1]);
	rc[2] = (lookat[2]-from[2]);
	distsq = rc[0]*rc[0] + rc[1]*rc[1] + rc[2]*rc[2];

	if (distsq > maxdistsq) {
		distsq = 0.15f * (distsq - maxdistsq) / distsq;
		
		from[0] += distsq*rc[0];
		from[1] += distsq*rc[1];
		from[2] += distsq*rc[2];
	}
	else if (distsq < mindistsq) {
		distsq = 0.15f * (mindistsq - distsq) / mindistsq;
		
		from[0] -= distsq*rc[0];
		from[1] -= distsq*rc[1];
		from[2] -= distsq*rc[2];
	}


	/* CONSTRAINT 7: track to floor below actor */
	rc[0] = (lookat[0]-from[0]);
	rc[1] = (lookat[1]-from[1]);
	rc[2] = (lookat[2]-from[2]);
	Kx_VecUpMat3(rc, mat, 3);	/* y up Track -z */
	



	/* now set the camera position and rotation */
	
	obj->NodeSetLocalPosition(from);
	
	actormat[0][0] = mat[0][0]; actormat[0][1] = mat[1][0]; actormat[0][2] = mat[2][0];
	actormat[1][0] = mat[0][1]; actormat[1][1] = mat[1][1]; actormat[1][2] = mat[2][1];
	actormat[2][0] = mat[0][2]; actormat[2][1] = mat[1][2]; actormat[2][2] = mat[2][2];
	obj->NodeSetLocalOrientation(actormat);

	return true;
}
Example #13
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;
}
Example #14
0
bool KX_CameraActuator::Update(double curtime, bool frame)
{
	/* wondering... is it really neccesary/desirable to suppress negative    */
	/* events here?                                                          */
	bool bNegativeEvent = IsNegativeEvent();
	RemoveAllEvents();

	if (bNegativeEvent || !m_ob) 
		return false;
	
	KX_GameObject *obj = (KX_GameObject*) GetParent();
	MT_Point3 from = obj->NodeGetWorldPosition();
	MT_Matrix3x3 frommat = obj->NodeGetWorldOrientation();
	/* These casts are _very_ dangerous!!! */
	MT_Point3 lookat = ((KX_GameObject*)m_ob)->NodeGetWorldPosition();
	MT_Matrix3x3 actormat = ((KX_GameObject*)m_ob)->NodeGetWorldOrientation();

	float fp1[3], fp2[3], rc[3];
	float inp, fac; //, factor = 0.0; /* some factor...                                    */
	float mindistsq, maxdistsq, distsq;
	float mat[3][3];
	
	/* The rules:                                                            */
	/* CONSTRAINT 1: not implemented */
	/* CONSTRAINT 2: can camera see actor?              */
	/* CONSTRAINT 3: fixed height relative to floor below actor.             */
	/* CONSTRAINT 4: camera rotates behind actor                              */
	/* CONSTRAINT 5: minimum / maximum distance                             */
	/* CONSTRAINT 6: again: fixed height relative to floor below actor        */
	/* CONSTRAINT 7: track to floor below actor                               */
	/* CONSTRAINT 8: look a little bit left or right, depending on how the

	   character is looking (horizontal x)
 */

	/* ...and then set the camera position. Since we assume the parent of    */
	/* this actuator is always a camera, just set the parent position and    */
	/* rotation. We do not check whether we really have a camera as parent.  */
	/* It may be better to turn this into a general tracking actuator later  */
	/* on, since lots of plausible relations can be filled in here.          */

	/* ... set up some parameters ...                                        */
	/* missing here: the 'floorloc' of the actor's shadow */

	mindistsq= m_minHeight*m_minHeight;
	maxdistsq= m_maxHeight*m_maxHeight;

	/* C1: not checked... is a future option                                 */

	/* C2: blender test_visibility function. Can this be a ray-test?         */

	/* C3: fixed height  */
	from[2] = (15.0*from[2] + lookat[2] + m_height)/16.0;


	/* C4: camera behind actor   */
	if (m_x) {
		fp1[0] = actormat[0][0];
		fp1[1] = actormat[1][0];
		fp1[2] = actormat[2][0];

		fp2[0] = frommat[0][0];
		fp2[1] = frommat[1][0];
		fp2[2] = frommat[2][0];
	} 
	else {
		fp1[0] = actormat[0][1];
		fp1[1] = actormat[1][1];
		fp1[2] = actormat[2][1];

		fp2[0] = frommat[0][1];
		fp2[1] = frommat[1][1];
		fp2[2] = frommat[2][1];
	}
	
	inp= fp1[0]*fp2[0] + fp1[1]*fp2[1] + fp1[2]*fp2[2];
	fac= (-1.0 + inp) * m_damping;

	from[0]+= fac*fp1[0];
	from[1]+= fac*fp1[1];
	from[2]+= fac*fp1[2];
	
	/* alleen alstie ervoor ligt: cross testen en loodrechte bijtellen */
	if(inp<0.0) {
		if(fp1[0]*fp2[1] - fp1[1]*fp2[0] > 0.0) {
			from[0]-= fac*fp1[1];
			from[1]+= fac*fp1[0];
		}
		else {
			from[0]+= fac*fp1[1];
			from[1]-= fac*fp1[0];
		}
	}

	/* CONSTRAINT 5: minimum / maximum afstand */

	rc[0]= (lookat[0]-from[0]);
	rc[1]= (lookat[1]-from[1]);
	rc[2]= (lookat[2]-from[2]);
	distsq= rc[0]*rc[0] + rc[1]*rc[1] + rc[2]*rc[2];

	if(distsq > maxdistsq) {
		distsq = 0.15*(distsq-maxdistsq)/distsq;
		
		from[0] += distsq*rc[0];
		from[1] += distsq*rc[1];
		from[2] += distsq*rc[2];
	}
	else if(distsq < mindistsq) {
		distsq = 0.15*(mindistsq-distsq)/mindistsq;
		
		from[0] -= distsq*rc[0];
		from[1] -= distsq*rc[1];
		from[2] -= distsq*rc[2];
	}


	/* CONSTRAINT 7: track to schaduw */
	rc[0]= (lookat[0]-from[0]);
	rc[1]= (lookat[1]-from[1]);
	rc[2]= (lookat[2]-from[2]);
	Kx_VecUpMat3(rc, mat, 3);	/* y up Track -z */
	



	/* now set the camera position and rotation */
	
	obj->NodeSetLocalPosition(from);
	
	actormat[0][0]= mat[0][0]; actormat[0][1]= mat[1][0]; actormat[0][2]= mat[2][0];
	actormat[1][0]= mat[0][1]; actormat[1][1]= mat[1][1]; actormat[1][2]= mat[2][1];
	actormat[2][0]= mat[0][2]; actormat[2][1]= mat[1][2]; actormat[2][2]= mat[2][2];
	obj->NodeSetLocalOrientation(actormat);

	return true;
}