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;
}
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);
}
}
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_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_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;
}
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) */
///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
}
}
}
}
}
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;
}
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);
}
}
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;
}
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;
}
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;
}
