Ejemplo n.º 1
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;
}
Ejemplo n.º 2
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;
}
Ejemplo n.º 3
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;
}