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