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