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;
}
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;
}
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;
}