// --------------------------------------------------------------------------
// ARDrone::move(X velocity[m/s], Y velocity[m/s], Rotational speed[rad/s])
// Description : Move the AR.Drone in 2D plane.
// Return value : NONE
// --------------------------------------------------------------------------
void ARDrone::move(double vx, double vy, double vr)
{
move3D(vx, vy, 0.0, vr);
}
// --------------------------------------------------------------------------
// ARDrone::followLine()
// Description : Send command to the AR Drone according to the line position
// Return value : True if a line was found, false elsewhere
// --------------------------------------------------------------------------
bool ARDrone::followLine(IplImage* image)
{
//Get the image of the bottom camera
const cv::Mat image_bottom (image, cv::Rect(0, 0, 85, 70));
//Useful parameters to move the drone
double offsetPix = 0.0, angleDeg = 0.0;
double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
const bool found = findLine(image_bottom, offsetPix, angleDeg, 43);
//If a line was found in the image
if (found)
{
//Ajust altitude
if (getAltitude() > 1.1)
vz = -0.5;
//Ajust angle
if (angleDeg < -10 || angleDeg > 10)
{
//Turn left
if (angleDeg < -10)
vr = 0.5;
//Turn right
else
vr = -0.5;
}
//Ajust according the line offset
else if (offsetPix < -20 || offsetPix > 20)
{
//Move left
if (offsetPix < -20)
vy = 0.25;
//Move right
else
vy = -0.25;
}
//If no ajustment is necessary, move on !
else
vx = 0.6;
//Send the command
move3D(vx, vy, vz, vr);
//sucess
return true;
}
//if no line was found, increase altitude to find a line
else if (getAltitude() < 1.2)
vz = 0.5;
//Send the command
move3D(vx, vy, vz, vr);
//Fail
return false;
}
void Camera3DTestDemo::updateCamera(float fDelta)
{
if(_sprite3D)
{
if( _cameraType==CameraType::ThirdPerson)
{
updateState(fDelta);
if(isState(_curState,State_Move))
{
move3D(fDelta);
if(isState(_curState,State_Rotate))
{
Vec3 curPos = _sprite3D->getPosition3D();
Vec3 newFaceDir = _targetPos - curPos;
newFaceDir.y = 0;
newFaceDir.normalize();
Vec3 up;
_sprite3D->getNodeToWorldTransform().getUpVector(&up);
up.normalize();
Vec3 right;
Vec3::cross(-newFaceDir,up,&right);
right.normalize();
Vec3 pos = Vec3(0,0,0);
Mat4 mat;
mat.m[0] = right.x;
mat.m[1] = right.y;
mat.m[2] = right.z;
mat.m[3] = 0.0f;
mat.m[4] = up.x;
mat.m[5] = up.y;
mat.m[6] = up.z;
mat.m[7] = 0.0f;
mat.m[8] = newFaceDir.x;
mat.m[9] = newFaceDir.y;
mat.m[10] = newFaceDir.z;
mat.m[11] = 0.0f;
mat.m[12] = pos.x;
mat.m[13] = pos.y;
mat.m[14] = pos.z;
mat.m[15] = 1.0f;
_sprite3D->setAdditionalTransform(&mat);
}
}
}
if(_bZoomOut == true)
{
if(_camera)
{
if(_cameraType == CameraType::ThirdPerson)
{
Vec3 lookDir = _camera->getPosition3D() - _sprite3D->getPosition3D();
Vec3 cameraPos = _camera->getPosition3D();
if(lookDir.length() <= 300)
{
cameraPos += lookDir.getNormalized();
_camera->setPosition3D(cameraPos);
}
}
else if(_cameraType == CameraType::Free)
{
Vec3 cameraPos = _camera->getPosition3D();
if(cameraPos.length() <= 300)
{
cameraPos += cameraPos.getNormalized();
_camera->setPosition3D(cameraPos);
}
}
}
}
if(_bZoomIn == true)
{
if(_camera)
{
if(_cameraType == CameraType::ThirdPerson)
{
Vec3 lookDir = _camera->getPosition3D() - _sprite3D->getPosition3D();
Vec3 cameraPos = _camera->getPosition3D();
if(lookDir.length() >= 50)
{
cameraPos -= lookDir.getNormalized();
_camera->setPosition3D(cameraPos);
}
}
else if(_cameraType == CameraType::Free)
{
Vec3 cameraPos = _camera->getPosition3D();
if(cameraPos.length() >= 50)
{
cameraPos -= cameraPos.getNormalized();
_camera->setPosition3D(cameraPos);
}
}
}
}
if(_bRotateLeft == true)
{
if(_cameraType==CameraType::Free || _cameraType==CameraType::FirstPerson)
{
Vec3 rotation3D= _camera->getRotation3D();
rotation3D.y+= 1;
_camera->setRotation3D(rotation3D);
}
}
if(_bRotateRight == true)
{
if(_cameraType==CameraType::Free || _cameraType==CameraType::FirstPerson)
{
Vec3 rotation3D= _camera->getRotation3D();
rotation3D.y-= 1;
_camera->setRotation3D(rotation3D);
}
}
}
}
