/**
* Projects a point in real world coordinates against the image
* Output: image coordinate in pixels
*/
CvPoint MultipleViewGeomOld::getProjectionOf(float angle, CvPoint3D32f point) {
//
map<float, CvMat*>::iterator iter = projMatList.find(angle);
CvMat *projMat = cvCreateMat(4, 3, CV_32FC1);
if (iter == projMatList.end()) {
// project matrix does not exist!!
// Calculate rotation matrix
CvMat* rtMat = calculateRotationMatrix(angle);
// Calculate projection matrix
projMat = calculateProjectionMatrix(rtMat);
projMat = cvCloneMat(projMat);
projMatList.insert(pair<float, CvMat*> (angle, projMat));
} else {
// otherwise it exists
projMat = iter->second;
}
LOG4CPLUS_DEBUG(myLogger,"Projection matrix for angle: " << radToDegree(angle) << " and points: " << point << endl << printCvMat(projMat));
// [u v 1] = proj * [X Y Z 1]
float uvContents[3];
//CvMat* uvMat = cvMat(3, 1, CV_32F, uvContents);
CvMat* uvMat = cvCreateMat(3, 1, CV_32F);
cvInitMatHeader(uvMat, 3, 1, CV_32F, uvContents);
float xyzContents[] = { point.x, point.y, point.z, 1 };
//CvMat* xyzMat = cvMat(4, 1, CV_32F, xyzContents);
CvMat* xyzMat = cvCreateMat(4, 1, CV_32F);
cvInitMatHeader(xyzMat, 4, 1, CV_32F, xyzContents);
cvMatMul (projMat, xyzMat,uvMat);
LOG4CPLUS_DEBUG(myLogger, "Result [u v 1] = proj * [X Y Z 1]: " << endl << printCvMat(uvMat));
return cvPoint(cvRound(cvmGet(uvMat, 0, 0)), cvRound(cvmGet(uvMat, 1, 0)));
}
void LogicManager::throwBall(float power, float rad)
{
assert(power >= 0 && rad >= 0);
float degree = radToDegree(rad);
float ratio;
float vx = 0;
float vy = 0;
if (degree <= 90)
{
ratio = degree/90;
vx = power*(1-ratio);
vy = -power*ratio;
}
else if (degree <= 180)
{
degree -= 90;
ratio = degree/90;
vx = -power*(ratio);
vy = -power*(1-ratio);
}
else if (degree <= 270)
{
degree -= 180;
ratio = degree/90;
vx = -power*(ratio);
vy = power*(1-ratio);
}
else if (degree <= 360)
{
degree -= 270;
ratio = degree/90;
vx = power*(ratio);
vy = power*(1-ratio);
}
this->ball.setVelocityX(vx);
this->ball.setVelocityY(vy);
this->ball.setAngle(degree);
}
float pixelToDeg( int pos, int maxPos, float maxDeg )
{
return radToDegree( pixelToRad( pos, maxPos, degreeToRad( maxDeg ) ) );
}
