void MarkerDetector::estimatePosition(std::vector<Marker>& detectedMarkers)
{
for (size_t i=0; i<detectedMarkers.size(); i++)
{
Marker& m = detectedMarkers[i];
cv::Mat Rvec;
cv::Mat_<float> Tvec;
cv::Mat raux,taux;
cv::solvePnP(m_markerCorners3d, m.points, camMatrix, distCoeff,raux,taux);
raux.convertTo(Rvec,CV_32F);
taux.convertTo(Tvec ,CV_32F);
cv::Mat_<float> rotMat(3,3);
cv::Rodrigues(Rvec, rotMat);
// Copy to transformation matrix
for (int col=0; col<3; col++)
{
for (int row=0; row<3; row++)
{
m.transformation.r().mat[row][col] = rotMat(row,col); // Copy rotation component
}
m.transformation.t().data[col] = Tvec(col); // Copy translation component
}
// Since solvePnP finds camera location, w.r.t to marker pose, to get marker pose w.r.t to the camera we invert it.
m.transformation = m.transformation.getInverted();
}
}
VectorXd LinearizedControlError::operator()(const VectorXd& a) const {
Vector6d x1 = a.topRows(6);
Vector6d x2 = a.middleRows(6,6);
double phi = a(12), Delta = a(13), curvature = a(14);
double phi_ref = cfg0->phi, Delta_ref = cfg0->Delta, curvature_ref = cfg0->curvature;
MatrixXd F = MatrixXd::Zero(6,6);
F.topLeftCorner(3, 3) = -rotMat(Vector3d(Delta_ref*curvature_ref, 0, phi_ref));
F.topRightCorner(3, 3) = -rotMat(Vector3d(0, 0, Delta_ref));
F.bottomRightCorner(3, 3) = -rotMat(Vector3d(Delta_ref*curvature_ref, 0, phi_ref));
MatrixXd G = MatrixXd::Zero(6,6);
G(2, 0) = 1;
G(3, 2) = 1;
G(5, 1) = 1;
MatrixXd A = F.exp();
MatrixXd halfF = 0.5*F;
MatrixXd B = (1/6.0) * (G + 4.0*(halfF.exp()*G) + A*G);
Vector3d u;
u << Delta - Delta_ref, phi - phi_ref, Delta*curvature - Delta_ref*curvature_ref;
return x2 - A * x1 - B * u;
}
void CybOBB::treatRotation(int axis, double angle)
{
double* mr;
double radAng = PI * angle / 180.0;
double seno = sin(radAng);
double cosseno = cos(radAng);
double mrx[9] = {1,0,0,0, cosseno, -seno, 0, seno, cosseno};
double mry[9] = {cosseno, 0, seno, 0, 1, 0, -seno, 0, cosseno};
double mrz[9] = {cosseno, -seno, 0, seno, cosseno, 0, 0, 0, 1};
if(axis == 0)mr = mrx;
else if(axis = 1) mr = mry;
else mr = mrz;
CybMatrix<double> rotMat(3,3,mr);
CybVector3D<double> verts[2];
verts[0](center[0] - sizes[0], center[1] - sizes[1], center[2] - sizes[2]);
verts[1](center[0] + sizes[0], center[1] + sizes[1], center[2] + sizes[2]);
for(int i = 0; i < 2; ++i){
CybMatrix<double> aux(verts[i]);
aux = aux * rotMat;
verts[i](aux[0][0], aux[0][1], aux[0][2]);
}
center = (verts[0] + verts[1])/2.0;
sizes = (verts[1] - verts[0])/2.0;
}
// --------------------------------------------------------------------------
//Does the actual rotation
bool psLinearMovement::RotateV (float delta)
{
if (!mesh)
{
return false;
}
// rotation
if (angularVelocity < SMALL_EPSILON)
return false;
csVector3 angle = angularVelocity * delta;
if (angleToReachFlag)
{
float current_yrot = GetYRotation();
float yrot_delta = fabs (angleToReach.y - current_yrot);
if (fabs(angle.y) > yrot_delta)
{
angle.y = (angle.y / fabs (angle.y)) * yrot_delta;
angularVelocity = 0;
angleToReachFlag = false;
}
}
iMovable* movable = mesh->GetMovable ();
csYRotMatrix3 rotMat (angle.y);
movable->SetTransform (movable->GetTransform ().GetT2O () * rotMat);
movable->UpdateMove ();
//pcmesh->GetMesh ()->GetMovable ()->Transform (rotMat);
return true;
}
void MarkerDetector::estimatePosition(Marker& m)
{
std::vector<cv::Point3f> m_3d;
m_3d.push_back(cv::Point3f(0,0,0));
m_3d.push_back(cv::Point3f(0,1,0));
m_3d.push_back(cv::Point3f(1,1,0));
m_3d.push_back(cv::Point3f(1,0,0));
//calculate 9 points of the marker
std::vector<cv::Point2f> imgPoints(4,0);
imgPoints[0] = m.points[0];
imgPoints[1] = m.points[1];
imgPoints[2] = m.points[3];
imgPoints[3] = m.points[2];
cv::Mat Rvec;
cv::Mat_<float> Tvec;
cv::Mat raux,taux;
cv::Mat_<float> rotMat(3,3);
cv::solvePnP(m_3d, imgPoints, camMatrix, distCoeff,raux,taux);
raux.convertTo(Rvec,CV_32F);
taux.convertTo(Tvec ,CV_32F);
std::cerr<<"NEW ROUND!"<<std::endl;
std::cerr<<"raux: "<<std::endl<<raux<<std::endl;
std::cerr<<"taux: "<<std::endl<<taux<<std::endl<<std::endl;
m.Tvec = Tvec;
m.Rvec = Rvec;
}
vec3f SceneObject::getWorldNormal(unsigned fi, const vec3f& position, bool flat) const
{
vec3f original_normal = SimpleShape::getWorldNormal(fi, position, flat);
if(bumpTex.size() <= 1 || fi >= faceVertexTexCoordIndexList.size())
return original_normal;
printf("use not original normal\n");
vec3f vps[3], vts[3], vns[3];
for(unsigned i=0; i<3; i++)
{
vps[i] = getWorldVertexPosition(faceVertexIndexList[fi][i]);
if(faceVertexTexCoordIndexList[fi][i] >= vertexTexCoordList.size())
return original_normal;
vts[i] = vertexTexCoordList[faceVertexTexCoordIndexList[fi][i]];
}
vec3f uv_grad = bumpTex.getGrad(getTexCoord(fi, position));
vec3f b1 = vps[1] - vps[0];
vec3f b2 = vps[2] - vps[0];
vec3f duv1 = vts[1] - vts[0];
vec3f duv2 = vts[2] - vts[0];
float k2 = (uv_grad.x*duv1.y - uv_grad.y*duv1.x) / (duv1.y*duv2.x - duv1.x*duv2.y);
float k1 = (uv_grad.y - k2*duv2.y) / duv1.y;
b1.normalize();
b2.normalize();
vec3f dl = k1*b1+k2*b2;
vec3f dh = original_normal*uv_grad.z;
if(dh.length()*1000 < dl.length())
return original_normal;
float angle = atan2(dh.length(), dl.length());
vec3f axis = dl.cross(dh);
axis.normalize();
return vec3f(rotMat(axis, angle) * vec4f(original_normal, 0));
}
void ENUUtil::compute( const double refLat,
const double refLon )
{
rotMat.resize(3,3);
rotMat (0,0) = -std::sin(refLon);
rotMat (1,0) = -std::sin(refLat)*std::cos(refLon);
rotMat (2,0) = std::cos(refLat)*std::cos(refLon);
rotMat (0,1) = std::cos(refLon);
rotMat (1,1) = -std::sin(refLat)*std::sin(refLon);
rotMat (2,1) = std::cos(refLat)*std::sin(refLon);
rotMat (0,2) = 0.0;
rotMat (1,2) = std::cos(refLat);
rotMat (2,2) = std::sin(refLat);
}
arma::mat33 ImuFilter::getMatrix() const {
arma::colvec4 q = getQuaternion();
arma::mat33 rotMat;
rotMat(0, 0) = 1-2*(q(2)*q(2) + q(3)*q(3));
rotMat(1, 1) = 1-2*(q(1)*q(1) + q(3)*q(3));
rotMat(2, 2) = 1-2*(q(1)*q(1) + q(2)*q(2));
rotMat(0, 1) = -2*q(0)*q(3) + 2*q(1)*q(2);
rotMat(0, 2) = 2*q(0)*q(2) + 2*q(1)*q(3);
rotMat(1, 0) = 2*q(0)*q(3) + 2*q(1)*q(2);
rotMat(1, 2) = -2*q(0)*q(1) + 2*q(2)*q(3);
rotMat(2, 0) = -2*q(0)*q(2) + 2*q(1)*q(3);
rotMat(2, 1) = 2*q(0)*q(1) + 2*q(2)*q(3);
return rotMat;
}
void DamnCute::Bullet::update(const glm::mat4& transform, sf::RenderTarget* w_ptr) {
_selfTransform *= transform;
glm::mat3 rotMat(_selfTransform);
glm::vec3 d(_selfTransform[3]);
glm::vec3 retVec = -d * rotMat;
_s.setPosition(retVec.x, retVec.y);
//updateQuadTreePos(retVec.x, retVec.y);
const sf::Rect<float>& r = _s.getGlobalBounds();
updateQuadTreePos(r.left, r.top, r.width, r.height);
w_ptr->draw(_s);
DamnCute::Core::getInstance()->addBulletsCounter();
}
void PatternTrackingInfo::computePose(const Pattern& pattern, const CameraCalibration& calibration)
{
cv::Mat raux,taux;
cv::solvePnP(pattern.points3d, points2d, calibration.getIntrinsic(), calibration.getDistorsion(),raux,taux);
raux.convertTo(Rvec,CV_32F);
taux.convertTo(Tvec ,CV_32F);
cv::Mat_<float> rotMat(3,3);
cv::Rodrigues(Rvec, rotMat);
// Copy to transformation matrix
for (int col=0; col<3; col++)
{
for (int row=0; row<3; row++)
{
pose3d.r().mat[row][col] = rotMat(row,col); // Copy rotation component
}
pose3d.t().data[col] = Tvec(col); // Copy translation component
}
// Since solvePnP finds camera location, w.r.t to marker pose, to get marker pose w.r.t to the camera we invert it.
pose3d = pose3d.getInverted();
}
void ClothEntity_cl::SetClothOrientation(const hkvVec3& vNewOri)
{
VThreadedTask *pTask = GetPreparationTask();
WAIT_UNTIL_FINISHED(pTask);
m_vCurrentOri = vNewOri;
hkvMat3 rotMat (hkvNoInitialization);
rotMat.setFromEulerAngles (vNewOri.z, vNewOri.y, vNewOri.x);
if (m_spMesh)
{
m_spMesh->Rotate(rotMat,m_vCurrentPos);
SetCurrentVisBoundingBox(m_spMesh->GetBoundingBox());
}
HandleAnimations(0.f); // force a vertex buffer update
}
glm::mat4 Camera::CalcLookAtMatrix()
{
glm::vec3 lookDir = glm::normalize(camTarget - camPos);
glm::vec3 upDir = glm::normalize(glm::vec3(upVector));
glm::vec3 rightDir = glm::normalize(glm::cross(lookDir, upDir));
glm::vec3 perpUpDir = glm::cross(rightDir, lookDir);
glm::mat4 rotMat(1.0f);
rotMat[0] = glm::vec4(rightDir, 0.0f);
rotMat[1] = glm::vec4(perpUpDir, 0.0f);
rotMat[2] = glm::vec4(-lookDir, 0.0f);
rotMat = glm::transpose(rotMat);
glm::mat4 transMat(1.0f);
transMat[3] = glm::vec4(-camPos, 1.0f);
return rotMat * transMat;
}
glm::mat4 GraphicsSubsystem::calcLookAtMatrix(const glm::vec3 &cameraPt, const glm::vec3 &lookPt, const glm::vec3 &upPt)
{
glm::vec3 lookDir = glm::normalize(lookPt - cameraPt);
glm::vec3 upDir = glm::normalize(upPt);
glm::vec3 rightDir = glm::normalize(glm::cross(lookDir, upDir));
glm::vec3 perpUpDir = glm::cross(rightDir, lookDir);
glm::mat4 rotMat(1.0f);
rotMat[0] = glm::vec4(rightDir, 0.0f);
rotMat[1] = glm::vec4(perpUpDir, 0.0f);
rotMat[2] = glm::vec4(-lookDir, 0.0f);
rotMat = glm::transpose(rotMat);
glm::mat4 transMat(1.0f);
transMat[3] = glm::vec4(-cameraPt, 1.0f);
return rotMat * transMat;
}
void OpenCV_Function::warpAffine() {
//【1】参数准备
//定义两组点，代表两个三角形
cv::Point2f srcTriangle[3];
cv::Point2f dstTriangle[3];
//定义一些Mat变量
cv::Mat rotMat( 2, 3, CV_32FC1 );
cv::Mat warpMat( 2, 3, CV_32FC1 );
//【2】加载源图像并作一些初始化
g_srcImage=cv::imread("girl-t1.jpg");
// 设置目标图像的大小和类型与源图像一致
g_resultImage = cv::Mat::zeros( g_srcImage.rows, g_srcImage.cols, g_srcImage.type() );
//【3】设置源图像和目标图像上的三组点以计算仿射变换
srcTriangle[0] = cv::Point2f( 0,0 );
srcTriangle[1] = cv::Point2f( static_cast<float>(g_srcImage.cols - 1), 0 );
srcTriangle[2] = cv::Point2f( 0, static_cast<float>(g_srcImage.rows - 1 ));
dstTriangle[0] = cv::Point2f( static_cast<float>(g_srcImage.cols*0.0), static_cast<float>(g_srcImage.rows*0.33));
dstTriangle[1] = cv::Point2f( static_cast<float>(g_srcImage.cols*0.65), static_cast<float>(g_srcImage.rows*0.35));
dstTriangle[2] = cv::Point2f( static_cast<float>(g_srcImage.cols*0.15), static_cast<float>(g_srcImage.rows*0.6));
//【4】求得仿射变换
warpMat = getAffineTransform( srcTriangle, dstTriangle );
//【5】对源图像应用刚刚求得的仿射变换
cv::warpAffine( g_srcImage, g_resultImage, warpMat, g_resultImage.size() );
//【6】对图像进行缩放后再旋转
cv::Point center = cv::Point( g_srcImage.cols/2, g_srcImage.rows/2 );
double angle = -20.0;
double scale = 1;
// 通过上面的旋转细节信息求得旋转矩阵
rotMat =cv::getRotationMatrix2D( center, angle, scale );
// 旋转已缩放后的图像
cv::warpAffine( g_resultImage, g_templateImage, rotMat, g_resultImage.size() );
cv::imshow("【原始图】",g_srcImage);
cv::imshow( "g_templateImage", g_templateImage );
cv::imshow( "g_resultImage", g_resultImage );
}
// -------------------------------------------------------------------------- //
// Overridden functions
// -------------------------------------------------------------------------- //
void VFmodCollisionMeshInstance::OnObject3DChanged(int iO3DFlags)
{
VisObject3D_cl::OnObject3DChanged(iO3DFlags);
if (!m_pManager->IsInitialized())
return;
if (m_pGeometry)
{
// update the transformation
const hkvVec3 &vPos = GetPosition();
hkvMat3 rotMat(hkvNoInitialization);
GetRotationMatrix(rotMat);
rotMat.transpose();
hkvVec3 vDir,vRight,vUp;
rotMat.getAxisXYZ(&vDir, &vRight, &vUp);
m_pGeometry->setRotation((FMOD_VECTOR*)&vUp, (FMOD_VECTOR*)&vRight);
m_pGeometry->setPosition((FMOD_VECTOR*)&vPos);
m_pGeometry->setScale((FMOD_VECTOR*)&GetScale());
}
}
void ARTracker::doEstimatePose(std::vector<ARMarker>& detectedMarkers){
for( size_t i=0; i<detectedMarkers.size(); i++ ){
ARMarker &marker = detectedMarkers.at(i);
cv::Mat Rvec;
cv::Mat_<float> Tvec;
cv::Mat raux,taux;
cv::solvePnP(_modelPoints, marker.corners, _intrinsicsMatrix, _distortionMatrix, raux, taux, false, CV_ITERATIVE);
raux.convertTo(Rvec,CV_32F);
taux.convertTo(Tvec ,CV_32F);
cv::Mat_<float> rotMat(3,3);
cv::Rodrigues(Rvec, rotMat);
cv::Mat modelMatrix;
cameraMatrixToOpenGL(modelMatrix, rotMat, Tvec);
marker.setModelMatrix(modelMatrix);
}
}
//*--------------------------------------------------------------------------*//
//* MAIN MAIN *//
//*--------------------------------------------------------------------------*//
int main(int argc, char* argv[])
{
// Initialise random number generator
srandom(time(NULL));
clock_t t0 = clock();
// Print header
printHeader();
// Read options
Options uo = parseCommandLine(argc,argv);
if(!uo.noHybrid)
{
if(uo.funcGroupVec[AROM] && uo.funcGroupVec[LIPO])
{
uo.funcGroupVec[HYBL] = true;
}
if(uo.funcGroupVec[HDON] && uo.funcGroupVec[HACC])
{
uo.funcGroupVec[HYBH] = true;
}
}
std::cerr << uo.print() << std::endl;
if (uo.version)
{
printHeader();
exit(0);
}
if (uo.help)
{
printUsage();
exit(0);
}
// Db file and pharmacophore out are mandatory elements
if (uo.dbInpFile.empty())
{
mainErr("Missing database file. This is a required option (-d).");
}
if (uo.pharmOutFile.empty() && uo.molOutFile.empty() && uo.scoreOutFile.empty())
{
mainErr("No output file defined. So there is actually no use to compute anything at all.");
}
if ((uo.pharmOutFile.empty() && uo.scoreOutFile.empty()) && !uo.molOutFile.empty())
{
mainErr("No file defined to write pharmacophore information.");
}
if (uo.refInpFile.empty() && uo.pharmOutFile.empty() && uo.molOutFile.empty() && !uo.scoreOutFile.empty())
{
mainErr("Only score file requested when no reference is given. Unable to generate this output.");
}
// Reference variables
Pharmacophore refPharm;
refPharm.clear();
std::string refId;
double refVolume(0.0);
int refSize(0);
int exclSize(0);
// Database variables
std::vector<Result*> resList;
Pharmacophore dbPharm;
std::string dbId;
double dbVolume(0.0);
int dbSize(0);
//----------------------------------------------------------------------------
//...(A).. Process the reference
//----------------------------------------------------------------------------
if (!uo.refInpFile.empty())
{
//-------------------------------------------------------
//...(1).. get reference pharmacophore
//-------------------------------------------------------
if (uo.refInpType == UNKNOWN)
{
std::string ext(getExt(uo.refInpFile));
if (ext == ".phar")
{
uo.refInpType = PHAR;
}
else
{
uo.refInpType = MOL;
}
}
if (uo.refInpType == MOL)
{
OpenBabel::OBMol m;
OpenBabel::OBConversion* reader = new OpenBabel::OBConversion();
reader->SetInFormat(reader->FormatFromExt(uo.refInpFile.c_str()));
if (!reader->Read(&m, uo.refInpStream))
{
mainErr("Unable to read reference molecule");
}
calcPharm(&m, &refPharm, uo);
refId = m.GetTitle();
delete reader;
reader = NULL;
}
else if (uo.refInpType == PHAR)
{
PharmacophoreReader* reader = new PharmacophoreReader();
refPharm = reader->read(uo.refInpStream, refId);
if (refPharm.empty())
{
mainErr("Error reading reference pharmacophore");
}
delete reader;
reader = NULL;
}
else
{
mainErr("Unknown format of reference molecule.");
}
//-------------------------------------------------------
//...(2).. process reference pharmacophore
//-------------------------------------------------------
if (uo.merge)
{
pharMerger.merge(refPharm);
}
refSize = refPharm.size();
for (unsigned int i(0); i < refSize; ++i)
{
if (refPharm[i].func == EXCL)
{
// extract overlap with exclusion spheres
for (unsigned int j(0); j < refPharm.size(); ++j)
{
if (refPharm[j].func != EXCL)
{
refVolume -= VolumeOverlap(refPharm[i], refPharm[j], !uo.noNormal);
}
}
exclSize++;
}
else
{
// add point self-overlap
refVolume += VolumeOverlap(refPharm[i], refPharm[i], !uo.noNormal);
}
}
if(!uo.isQuiet)
{
std::cerr << "Reference pharmacophore " << refId << std::endl;
std::cerr << " number of points: " << refSize - exclSize << std::endl;
std::cerr << " number of exclusion spheres: " << exclSize << std::endl;
std::cerr << " totalvolume: " << refVolume << std::endl;
}
}
//----------------------------------------------------------------------------
//...(B).. Process the database file
//----------------------------------------------------------------------------
// DB files
if (uo.dbInpType == UNKNOWN)
{
std::string ext(getExt(uo.dbInpFile));
if (ext==".phar")
{
uo.dbInpType = PHAR;
}
else
{
uo.dbInpType = MOL;
}
}
// local storage of the rotation matrix
SiMath::Matrix rotMat(3,3,0.0);
unsigned int molCount(0);
OpenBabel::OBConversion* molReader = NULL;
PharmacophoreReader* pharmReader = NULL;
if (uo.dbInpType == PHAR)
{
pharmReader = new PharmacophoreReader();
}
else if (uo.dbInpType == MOL)
{
molReader = new OpenBabel::OBConversion();
molReader->SetInFormat(molReader->FormatFromExt(uo.dbInpFile.c_str()));
molReader->SetInStream(uo.dbInpStream);
}
else
{
mainErr("Unknown format of db file.");
}
bool done(false);
OpenBabel::OBMol m;
while (!done)
{
dbPharm.clear();
m.Clear();
if (uo.dbInpType == MOL)
{
if (!molReader->Read(&m))
{
done = true;
break;
}
else
{
calcPharm(&m, &dbPharm, uo);
dbId = m.GetTitle();
}
}
else
{
if (uo.dbInpStream->eof())
{
done = true;
break;
}
else
{
dbPharm = pharmReader->read(uo.dbInpStream, dbId);
}
}
if (dbPharm.empty())
{
continue;
}
++molCount;
if (!uo.isQuiet )
{
if ((molCount % 10) == 0)
{
std::cerr << "." << std::flush;
}
if ((molCount % 500) == 0)
{
std::cerr << molCount << std::endl << std::flush;
}
}
if (uo.merge)
{
pharMerger.merge(dbPharm);
}
if (uo.refInpFile.empty())
{
if (!(uo.isQuiet))
{
printProgress(molCount);
}
if( !uo.pharmOutFile.empty())
{
uo.pharmOutWriter->write(dbPharm, uo.pharmOutStream, dbId);
}
continue;
}
//-------------------------------------------------------
//...(1).. Alignment
//-------------------------------------------------------
dbSize = dbPharm.size();
dbVolume = 0.0;
for (unsigned int i(0); i < dbSize; ++i)
{
if (dbPharm[i].func == EXCL)
{
continue;
}
dbVolume += VolumeOverlap(dbPharm[i], dbPharm[i], !uo.noNormal);
}
// Create a result structure
Result res;
res.refId = refId;
res.refVolume = refVolume;
res.dbId = dbId;
res.dbVolume = dbVolume;
res.overlapVolume = 0.0;
res.exclVolume = 0.0;
res.resMol = m;
res.resPharSize = 0;
if (uo.scoreOnly)
{
FunctionMapping funcMap(&refPharm, &dbPharm, uo.epsilon);
PharmacophoreMap fMap = funcMap.getNextMap();
double volBest(-9999.999);
// loop over all reference points
while (!fMap.empty())
{
double newVol(0.0);
double exclVol(0.0);
for (PharmacophoreMap::iterator itP = fMap.begin(); itP != fMap.end(); ++itP)
{
if ((itP->first)->func == EXCL)
{
exclVol += VolumeOverlap((itP->first), (itP->second), !uo.noNormal);
}
else if (((itP->first)->func == (itP->second)->func ) ||
(((itP->first)->func == HYBH ||
(itP->first)->func == HDON ||
(itP->first)->func == HACC)
&& ((itP->second)->func == HDON ||
(itP->second)->func == HACC ||
(itP->second)->func == HYBH))
|| (((itP->first)->func == HYBL ||
(itP->first)->func == AROM ||
(itP->first)->func == LIPO)
&& ((itP->second)->func == AROM ||
(itP->second)->func == LIPO ||
(itP->second)->func == HYBL)))
{
newVol += VolumeOverlap((itP->first),(itP->second), !uo.noNormal);
}
}
if ((newVol - exclVol) > volBest)
{
res.resPhar.clear();
res.resPharSize = 0;
for (PharmacophoreMap::iterator itP = fMap.begin(); itP != fMap.end(); ++itP)
{
// add point to resulting pharmacophore
PharmacophorePoint p(itP->second);
(res.resPhar).push_back(p);
++res.resPharSize;
}
res.overlapVolume = newVol;
res.exclVolume = exclVol;
volBest = newVol - exclVol;
}
// get the next map
fMap.clear();
fMap = funcMap.getNextMap();
}
}
else
{
FunctionMapping funcMap(&refPharm, &dbPharm, uo.epsilon);
PharmacophoreMap fMap = funcMap.getNextMap();
PharmacophoreMap bestMap;
// default solution
SolutionInfo best;
best.volume = -999.9;
// rotor is set to no rotation
best.rotor.resize(4);
best.rotor = 0.0;
best.rotor[0] = 1.0;
double bestScore = -1000;
int mapSize(fMap.size());
int maxSize = mapSize - 3;
while (!fMap.empty())
{
int msize = fMap.size();
// add the exclusion spheres to the alignment procedure
if (uo.withExclusion)
{
for (unsigned int i(0); i < refSize ; ++i)
{
if (refPharm[i].func != EXCL)
{
continue;
}
for (unsigned int j(0); j < dbSize; ++j)
{
if (dbPharm[j].func == EXCL)
{
continue;
}
fMap.insert(std::make_pair(&(refPharm[i]), &(dbPharm[j])));
}
}
}
// Only align if the expected score has any chance of being larger
// than best score so far
if ((msize > maxSize)
&& (((double) msize / (refSize - exclSize + dbSize - msize)) > bestScore))
{
Alignment align(fMap);
SolutionInfo r = align.align(!uo.noNormal);
if (best.volume < r.volume)
{
best = r;
bestScore = best.volume / (refVolume + dbVolume - best.volume);
bestMap = fMap;
mapSize = msize;
}
}
else
{
// Level of mapping site to low
break;
}
if (bestScore > 0.98)
{
break;
}
// Get the next map
fMap.clear();
fMap = funcMap.getNextMap();
}
// Transform the complete pharmacophore and the molecule towards the
// best alignment
rotMat = quat2Rotation(best.rotor);
positionPharmacophore(dbPharm, rotMat, best);
positionMolecule(&res.resMol, rotMat, best);
// Update result
res.info = best;
// Compute overlap volume between exlusion spheres and pharmacophore
// points
for (int i(0); i < refSize; ++i)
{
if (refPharm[i].func != EXCL)
{
continue;
}
for (int j(0); j < dbSize; ++j)
{
res.exclVolume += VolumeOverlap(refPharm[i], dbPharm[j], !uo.noNormal);
}
}
// make copy of the best map and compute the volume overlap
for (PharmacophoreMap::iterator itP = bestMap.begin(); itP != bestMap.end(); ++itP)
{
if(((itP->first)->func == EXCL) || ((itP->second)->func == EXCL))
{
continue;
}
// compute overlap
res.overlapVolume += VolumeOverlap(itP->first, itP->second, !uo.noNormal);
// add point to resulting pharmacophore
PharmacophorePoint p(itP->second);
(res.resPhar).push_back(p);
++res.resPharSize;
}
}
// update scores
res.info.volume = res.overlapVolume - res.exclVolume;
if (res.info.volume > 0.0)
{
res.tanimoto = res.info.volume / (res.refVolume + res.dbVolume - res.info.volume);
res.tversky_ref = res.info.volume / res.refVolume;
res.tversky_db = res.info.volume / res.dbVolume;
}
switch (uo.rankby)
{
case TANIMOTO:
res.rankbyScore = res.tanimoto;
break;
case TVERSKY_REF:
res.rankbyScore = res.tversky_ref;
break;
case TVERSKY_DB:
res.rankbyScore = res.tversky_db;
break;
}
//-------------------------------------------------------
//...(5).. Generate output
//-------------------------------------------------------
if (uo.cutOff != 0.0)
{
if (res.rankbyScore < uo.cutOff)
{
continue;
}
}
if (uo.best != 0)
{
addBest(res, uo, resList);
}
else
{
if (!uo.molOutFile.empty())
{
logOut(&res, uo);
}
if (!uo.pharmOutFile.empty())
{
logPharmacophores(&res, uo);
}
if (!uo.scoreOutFile.empty())
{
logScores(&res, uo);
}
}
}
if (molReader)
{
delete molReader;
molReader = NULL;
}
if (pharmReader)
{
delete pharmReader;
pharmReader = NULL;
}
//----------------------------------------------------------------------------
//...(C).. Process best list (if defined)
//----------------------------------------------------------------------------
if (uo.best != 0)
{
std::vector<Result*>::iterator itR;
for (itR = resList.begin(); itR != resList.end(); ++itR)
{
Result* res(*itR);
if (!uo.molOutFile.empty())
{
logOut(res, uo);
}
if (!uo.pharmOutFile.empty())
{
logPharmacophores(res, uo);
}
if (!uo.scoreOutFile.empty())
{
logScores(res, uo);
}
delete res;
}
}
// done processing database
if (!uo.isQuiet)
{
if (uo.refInpFile.empty())
{
std::cerr << std::endl;
std::cerr << "Processed " << molCount << " molecules";
double tt = (double)(clock() - t0 )/CLOCKS_PER_SEC;
std::cerr << " in " << tt << " seconds (";
std::cerr << molCount/tt << " molecules per second)." << std::endl;
}
else
{
std::cerr << std::endl;
std::cerr << "Processed " << molCount << " molecules" << std::endl;
double tt = (double)(clock() - t0 )/CLOCKS_PER_SEC;
std::cerr << molCount << " alignments in " << tt << " seconds (";
std::cerr << molCount/tt << " alignments per second)." << std::endl;
}
}
exit(0);
}
void GuiTSCtrl::onRender(Point2I offset, const RectI &updateRect)
{
// Save the current transforms so we can restore
// it for child control rendering below.
GFXTransformSaver saver;
bool renderingToTarget = false;
if(!processCameraQuery(&mLastCameraQuery))
{
// We have no camera, but render the GUI children
// anyway. This makes editing GuiTSCtrl derived
// controls easier in the GuiEditor.
renderChildControls( offset, updateRect );
return;
}
GFXTargetRef origTarget = GFX->getActiveRenderTarget();
// Set up the appropriate render style
U32 prevRenderStyle = GFX->getCurrentRenderStyle();
Point2F prevProjectionOffset = GFX->getCurrentProjectionOffset();
Point2I renderSize = getExtent();
if(mRenderStyle == RenderStyleStereoSideBySide)
{
GFX->setCurrentRenderStyle(GFXDevice::RS_StereoSideBySide);
GFX->setCurrentProjectionOffset(mLastCameraQuery.projectionOffset);
GFX->setStereoEyeOffsets(mLastCameraQuery.eyeOffset);
if (!mLastCameraQuery.hasStereoTargets)
{
// Need to calculate our current viewport here
mLastCameraQuery.stereoViewports[0] = updateRect;
mLastCameraQuery.stereoViewports[0].extent.x /= 2;
mLastCameraQuery.stereoViewports[1] = mLastCameraQuery.stereoViewports[0];
mLastCameraQuery.stereoViewports[1].point.x += mLastCameraQuery.stereoViewports[1].extent.x;
}
if (!mLastCameraQuery.hasFovPort)
{
// Need to make our own fovPort
mLastCameraQuery.fovPort[0] = CalculateFovPortForCanvas(mLastCameraQuery.stereoViewports[0], mLastCameraQuery);
mLastCameraQuery.fovPort[1] = CalculateFovPortForCanvas(mLastCameraQuery.stereoViewports[1], mLastCameraQuery);
}
GFX->setStereoFovPort(mLastCameraQuery.fovPort); // NOTE: this specifies fov for BOTH eyes
GFX->setSteroViewports(mLastCameraQuery.stereoViewports);
GFX->setStereoTargets(mLastCameraQuery.stereoTargets);
MatrixF myTransforms[2];
if (smUseLatestDisplayTransform)
{
// Use the view matrix determined from the display device
myTransforms[0] = mLastCameraQuery.eyeTransforms[0];
myTransforms[1] = mLastCameraQuery.eyeTransforms[1];
}
else
{
// Use the view matrix determined from the control object
myTransforms[0] = mLastCameraQuery.cameraMatrix;
myTransforms[1] = mLastCameraQuery.cameraMatrix;
QuatF qrot = mLastCameraQuery.cameraMatrix;
Point3F pos = mLastCameraQuery.cameraMatrix.getPosition();
Point3F rotEyePos;
myTransforms[0].setPosition(pos + qrot.mulP(mLastCameraQuery.eyeOffset[0], &rotEyePos));
myTransforms[1].setPosition(pos + qrot.mulP(mLastCameraQuery.eyeOffset[1], &rotEyePos));
}
GFX->setStereoEyeTransforms(myTransforms);
// Allow render size to originate from the render target
if (mLastCameraQuery.stereoTargets[0])
{
renderSize = mLastCameraQuery.stereoViewports[0].extent;
renderingToTarget = true;
}
}
else
{
GFX->setCurrentRenderStyle(GFXDevice::RS_Standard);
}
if ( mReflectPriority > 0 )
{
// Get the total reflection priority.
F32 totalPriority = 0;
for ( U32 i=0; i < smAwakeTSCtrls.size(); i++ )
if ( smAwakeTSCtrls[i]->isVisible() )
totalPriority += smAwakeTSCtrls[i]->mReflectPriority;
REFLECTMGR->update( mReflectPriority / totalPriority,
getExtent(),
mLastCameraQuery );
}
if(mForceFOV != 0)
mLastCameraQuery.fov = mDegToRad(mForceFOV);
if(mCameraZRot)
{
MatrixF rotMat(EulerF(0, 0, mDegToRad(mCameraZRot)));
mLastCameraQuery.cameraMatrix.mul(rotMat);
}
Frustum frustum;
if(mRenderStyle == RenderStyleStereoSideBySide)
{
// NOTE: these calculations are essentially overridden later by the fov port settings when rendering each eye.
MathUtils::makeFovPortFrustum(&frustum, mLastCameraQuery.ortho, mLastCameraQuery.nearPlane, mLastCameraQuery.farPlane, mLastCameraQuery.fovPort[0]);
}
else
{
// set up the camera and viewport stuff:
F32 wwidth;
F32 wheight;
F32 renderWidth = F32(renderSize.x);
F32 renderHeight = F32(renderSize.y);
F32 aspectRatio = renderWidth / renderHeight;
// Use the FOV to calculate the viewport height scale
// then generate the width scale from the aspect ratio.
if(!mLastCameraQuery.ortho)
{
wheight = mLastCameraQuery.nearPlane * mTan(mLastCameraQuery.fov / 2.0f);
wwidth = aspectRatio * wheight;
}
else
{
wheight = mLastCameraQuery.fov;
wwidth = aspectRatio * wheight;
}
F32 hscale = wwidth * 2.0f / renderWidth;
F32 vscale = wheight * 2.0f / renderHeight;
F32 left = (updateRect.point.x - offset.x) * hscale - wwidth;
F32 right = (updateRect.point.x + updateRect.extent.x - offset.x) * hscale - wwidth;
F32 top = wheight - vscale * (updateRect.point.y - offset.y);
F32 bottom = wheight - vscale * (updateRect.point.y + updateRect.extent.y - offset.y);
frustum.set( mLastCameraQuery.ortho, left, right, top, bottom, mLastCameraQuery.nearPlane, mLastCameraQuery.farPlane );
}
// Manipulate the frustum for tiled screenshots
const bool screenShotMode = gScreenShot && gScreenShot->isPending();
if ( screenShotMode )
{
gScreenShot->tileFrustum( frustum );
GFX->setViewMatrix(MatrixF::Identity);
}
RectI tempRect = updateRect;
if (!renderingToTarget)
{
#ifdef TORQUE_OS_MAC
Point2I screensize = getRoot()->getWindowSize();
tempRect.point.y = screensize.y - (tempRect.point.y + tempRect.extent.y);
#endif
GFX->setViewport( tempRect );
}
else
{
// Activate stereo RT
GFX->activateStereoTarget(-1);
}
// Clear the zBuffer so GUI doesn't hose object rendering accidentally
GFX->clear( GFXClearZBuffer , ColorI(20,20,20), 1.0f, 0 );
//GFX->clear( GFXClearTarget, ColorI(255,0,0), 1.0f, 0);
GFX->setFrustum( frustum );
if(mLastCameraQuery.ortho)
{
mOrthoWidth = frustum.getWidth();
mOrthoHeight = frustum.getHeight();
}
// We're going to be displaying this render at size of this control in
// pixels - let the scene know so that it can calculate e.g. reflections
// correctly for that final display result.
gClientSceneGraph->setDisplayTargetResolution(renderSize);
// Set the GFX world matrix to the world-to-camera transform, but don't
// change the cameraMatrix in mLastCameraQuery. This is because
// mLastCameraQuery.cameraMatrix is supposed to contain the camera-to-world
// transform. In-place invert would save a copy but mess up any GUIs that
// depend on that value.
MatrixF worldToCamera = mLastCameraQuery.cameraMatrix;
worldToCamera.inverse();
GFX->setWorldMatrix( worldToCamera );
mSaveProjection = GFX->getProjectionMatrix();
mSaveModelview = GFX->getWorldMatrix();
mSaveViewport = updateRect;
mSaveWorldToScreenScale = GFX->getWorldToScreenScale();
mSaveFrustum = GFX->getFrustum();
mSaveFrustum.setTransform( mLastCameraQuery.cameraMatrix );
// Set the default non-clip projection as some
// objects depend on this even in non-reflect cases.
gClientSceneGraph->setNonClipProjection( mSaveProjection );
// Give the post effect manager the worldToCamera, and cameraToScreen matrices
PFXMGR->setFrameMatrices( mSaveModelview, mSaveProjection );
renderWorld(updateRect);
DebugDrawer::get()->render();
// Render the canvas overlay if its available
if (mRenderStyle == RenderStyleStereoSideBySide && mStereoGuiTarget.getPointer())
{
GFXDEBUGEVENT_SCOPE( StereoGui_Render, ColorI( 255, 0, 0 ) );
MatrixF proj(1);
Frustum originalFrustum = GFX->getFrustum();
GFXTextureObject *texObject = mStereoGuiTarget->getTexture(0);
const FovPort *currentFovPort = GFX->getStereoFovPort();
const MatrixF *eyeTransforms = GFX->getStereoEyeTransforms();
const Point3F *eyeOffset = GFX->getStereoEyeOffsets();
Frustum gfxFrustum = originalFrustum;
for (U32 i=0; i<2; i++)
{
GFX->activateStereoTarget(i);
MathUtils::makeFovPortFrustum(&gfxFrustum, true, gfxFrustum.getNearDist(), gfxFrustum.getFarDist(), currentFovPort[i], eyeTransforms[i]);
GFX->setFrustum(gfxFrustum);
MatrixF eyeWorldTrans(1);
eyeWorldTrans.setPosition(Point3F(eyeOffset[i].x,eyeOffset[i].y,eyeOffset[i].z));
MatrixF eyeWorld(1);
eyeWorld.mul(eyeWorldTrans);
eyeWorld.inverse();
GFX->setWorldMatrix(eyeWorld);
GFX->setViewMatrix(MatrixF::Identity);
if (!mStereoOverlayVB.getPointer())
{
mStereoOverlayVB.set(GFX, 4, GFXBufferTypeStatic);
GFXVertexPCT *verts = mStereoOverlayVB.lock(0, 4);
F32 texLeft = 0.0f;
F32 texRight = 1.0f;
F32 texTop = 1.0f;
F32 texBottom = 0.0f;
F32 rectRatio = gfxFrustum.getWidth() / gfxFrustum.getHeight();
F32 rectWidth = gfxFrustum.getWidth() * TS_OVERLAY_SCREEN_WIDTH;
F32 rectHeight = rectWidth * rectRatio;
F32 screenLeft = -rectWidth * 0.5;
F32 screenRight = rectWidth * 0.5;
F32 screenTop = -rectHeight * 0.5;
F32 screenBottom = rectHeight * 0.5;
const F32 fillConv = 0.0f;
const F32 frustumDepthAdjusted = gfxFrustum.getNearDist() + 0.012;
verts[0].point.set( screenLeft - fillConv, frustumDepthAdjusted, screenTop - fillConv );
verts[1].point.set( screenRight - fillConv, frustumDepthAdjusted, screenTop - fillConv );
verts[2].point.set( screenLeft - fillConv, frustumDepthAdjusted, screenBottom - fillConv );
verts[3].point.set( screenRight - fillConv, frustumDepthAdjusted, screenBottom - fillConv );
verts[0].color = verts[1].color = verts[2].color = verts[3].color = ColorI(255,255,255,255);
verts[0].texCoord.set( texLeft, texTop );
verts[1].texCoord.set( texRight, texTop );
verts[2].texCoord.set( texLeft, texBottom );
verts[3].texCoord.set( texRight, texBottom );
mStereoOverlayVB.unlock();
}
if (!mStereoGuiSB.getPointer())
{
// DrawBitmapStretchSR
GFXStateBlockDesc bitmapStretchSR;
bitmapStretchSR.setCullMode(GFXCullNone);
bitmapStretchSR.setZReadWrite(false, false);
bitmapStretchSR.setBlend(true, GFXBlendSrcAlpha, GFXBlendInvSrcAlpha);
bitmapStretchSR.samplersDefined = true;
bitmapStretchSR.samplers[0] = GFXSamplerStateDesc::getClampLinear();
bitmapStretchSR.samplers[0].minFilter = GFXTextureFilterPoint;
bitmapStretchSR.samplers[0].mipFilter = GFXTextureFilterPoint;
bitmapStretchSR.samplers[0].magFilter = GFXTextureFilterPoint;
mStereoGuiSB = GFX->createStateBlock(bitmapStretchSR);
}
GFX->setVertexBuffer(mStereoOverlayVB);
GFX->setStateBlock(mStereoGuiSB);
GFX->setTexture( 0, texObject );
GFX->setupGenericShaders( GFXDevice::GSModColorTexture );
GFX->drawPrimitive( GFXTriangleStrip, 0, 2 );
}
}
// Restore the previous matrix state before
// we begin rendering the child controls.
saver.restore();
// Restore the render style and any stereo parameters
GFX->setActiveRenderTarget(origTarget);
GFX->setCurrentRenderStyle(prevRenderStyle);
GFX->setCurrentProjectionOffset(prevProjectionOffset);
if(mRenderStyle == RenderStyleStereoSideBySide && gLastStereoTexture)
{
GFX->setClipRect(updateRect);
GFX->getDrawUtil()->drawBitmapStretch(gLastStereoTexture, updateRect);
}
// Allow subclasses to render 2D elements.
GFX->setClipRect(updateRect);
renderGui( offset, updateRect );
if (shouldRenderChildControls())
{
renderChildControls(offset, updateRect);
}
smFrameCount++;
}
void GLWidget::paintGL(){
glPushAttrib(GL_ALL_ATTRIB_BITS);
glClearColor(m_clearcolor.red()/255.0,m_clearcolor.green()/255.0,m_clearcolor.blue()/255.0,1);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
//ground pre
if(m_displayGround){
glDepthMask(false);
m_objectShader.apply(PG::UTIL::mat4(), viewMatrix, perspectiveMatrix);
glActiveTexture(GL_TEXTURE0);
m_groundTexture.apply();
m_groundGeometry.apply();
glDepthMask(true);
}
if(m_animationInfo){
//PG_INFO_STREAM("RENDER!");
//sprite
modelMatrix = PG::UTIL::mat4();
glDepthFunc(GL_ALWAYS);
//glDepthMask(false);
for(const Layer* lay: m_animationInfo.getCurrentAnimation()->getLayers()){
//PG_INFO_STREAM("Render Layer '"<<lay->getName().toStdString()<<"'!");
const Keyframe* keyframe = m_animationInfo.getCurrentKeyframe(lay);
if(keyframe){
if(!m_displayExternalReferences && m_spriteSheet->getCutouts()[keyframe->getCutoutID()]->isExternalSheet())
continue;
//if(keyframe->isSelected()) continue;
//PG_INFO_STREAM("Render Keyframe!");
m_animationInfo.setCurrentModelMat(modelMatrix, keyframe);
m_spriteShader.apply(modelMatrix, viewMatrix, perspectiveMatrix);
m_animationInfo.setUniforms(m_spriteShader, keyframe);
m_animationInfo.apply(keyframe);
m_spriteGeometry.apply();
if(keyframe->isSelected()){
//glDisable(GL_DEPTH_TEST);
m_lineShader.apply(modelMatrix, viewMatrix, perspectiveMatrix);
m_spriteOutline.apply();
PG::UTIL::mat4 anchorModelMatrix = PG::UTIL::translation(keyframe->getOffsetX()/50.0f, -keyframe->getOffsetY()/50.0f,0.0f)*PG::UTIL::scale(0.2f,0.2f,0.2f)*PG::UTIL::translation(-0.5f, 0.5f,0.0f);
m_displayShader.apply(anchorModelMatrix, viewMatrix, perspectiveMatrix);
glActiveTexture(GL_TEXTURE0);
m_anchorTexture.apply();
m_spriteGeometry.apply();
//glEnable(GL_DEPTH_TEST);
}
}
}
glDepthFunc(GL_LEQUAL);
//glDepthMask(true);
}
//ground
if(m_displayGround){
m_objectShader.apply(PG::UTIL::mat4(), viewMatrix, perspectiveMatrix);
glActiveTexture(GL_TEXTURE0);
m_groundTexture.apply();
m_groundGeometry.apply();
}
if(m_animationInfo){
//shadow
if(m_displayShadow){
modelMatrix = PG::UTIL::mat4();
modelMatrix[0][0] = 0.5;
modelMatrix[1][1] = 0.5;
modelMatrix[2][2] = 0.5;
modelMatrix[3][0] = -0.2;
modelMatrix[3][1] = 0.02;
modelMatrix[3][2] = -0.2;
modelMatrix[3][3] = 1;
PG::UTIL::mat4 rotMat(0);
rotMat[0][0] = 1;
rotMat[1][2] = -1;
rotMat[2][1] = 1;
rotMat[3][3] = 1;
modelMatrix = modelMatrix*rotMat;
m_objectShader.apply(modelMatrix, viewMatrix, perspectiveMatrix);
glActiveTexture(GL_TEXTURE0);
m_shadowTexture.apply();
m_spriteGeometry.apply();
}
}
//clean up
m_spriteShader.release();
m_spriteGeometry.release();
glPopAttrib();
}
void GuiTSCtrl::onRender(Point2I offset, const RectI &updateRect)
{
// Save the current transforms so we can restore
// it for child control rendering below.
GFXTransformSaver saver;
if(!processCameraQuery(&mLastCameraQuery))
{
// We have no camera, but render the GUI children
// anyway. This makes editing GuiTSCtrl derived
// controls easier in the GuiEditor.
renderChildControls( offset, updateRect );
return;
}
// Set up the appropriate render style
U32 prevRenderStyle = GFX->getCurrentRenderStyle();
Point2F prevProjectionOffset = GFX->getCurrentProjectionOffset();
Point3F prevEyeOffset = GFX->getStereoEyeOffset();
if(mRenderStyle == RenderStyleStereoSideBySide)
{
GFX->setCurrentRenderStyle(GFXDevice::RS_StereoSideBySide);
GFX->setCurrentProjectionOffset(mLastCameraQuery.projectionOffset);
GFX->setStereoEyeOffset(mLastCameraQuery.eyeOffset);
}
else
{
GFX->setCurrentRenderStyle(GFXDevice::RS_Standard);
}
if ( mReflectPriority > 0 )
{
// Get the total reflection priority.
F32 totalPriority = 0;
for ( U32 i=0; i < smAwakeTSCtrls.size(); i++ )
if ( smAwakeTSCtrls[i]->isVisible() )
totalPriority += smAwakeTSCtrls[i]->mReflectPriority;
REFLECTMGR->update( mReflectPriority / totalPriority,
getExtent(),
mLastCameraQuery );
}
if(mForceFOV != 0)
mLastCameraQuery.fov = mDegToRad(mForceFOV);
if(mCameraZRot)
{
MatrixF rotMat(EulerF(0, 0, mDegToRad(mCameraZRot)));
mLastCameraQuery.cameraMatrix.mul(rotMat);
}
// set up the camera and viewport stuff:
F32 wwidth;
F32 wheight;
F32 renderWidth = (mRenderStyle == RenderStyleStereoSideBySide) ? F32(getWidth())*0.5f : F32(getWidth());
F32 renderHeight = F32(getHeight());
F32 aspectRatio = renderWidth / renderHeight;
// Use the FOV to calculate the viewport height scale
// then generate the width scale from the aspect ratio.
if(!mLastCameraQuery.ortho)
{
wheight = mLastCameraQuery.nearPlane * mTan(mLastCameraQuery.fov / 2.0f);
wwidth = aspectRatio * wheight;
}
else
{
wheight = mLastCameraQuery.fov;
wwidth = aspectRatio * wheight;
}
F32 hscale = wwidth * 2.0f / renderWidth;
F32 vscale = wheight * 2.0f / renderHeight;
Frustum frustum;
if(mRenderStyle == RenderStyleStereoSideBySide)
{
F32 left = 0.0f * hscale - wwidth;
F32 right = renderWidth * hscale - wwidth;
F32 top = wheight - vscale * 0.0f;
F32 bottom = wheight - vscale * renderHeight;
frustum.set( mLastCameraQuery.ortho, left, right, top, bottom, mLastCameraQuery.nearPlane, mLastCameraQuery.farPlane );
}
else
{
F32 left = (updateRect.point.x - offset.x) * hscale - wwidth;
F32 right = (updateRect.point.x + updateRect.extent.x - offset.x) * hscale - wwidth;
F32 top = wheight - vscale * (updateRect.point.y - offset.y);
F32 bottom = wheight - vscale * (updateRect.point.y + updateRect.extent.y - offset.y);
frustum.set( mLastCameraQuery.ortho, left, right, top, bottom, mLastCameraQuery.nearPlane, mLastCameraQuery.farPlane );
}
// Manipulate the frustum for tiled screenshots
const bool screenShotMode = gScreenShot && gScreenShot->isPending();
if ( screenShotMode )
{
gScreenShot->tileFrustum( frustum );
GFX->setViewMatrix(MatrixF::Identity);
}
RectI tempRect = updateRect;
#ifdef TORQUE_OS_MAC
Point2I screensize = getRoot()->getWindowSize();
tempRect.point.y = screensize.y - (tempRect.point.y + tempRect.extent.y);
#endif
GFX->setViewport( tempRect );
// Clear the zBuffer so GUI doesn't hose object rendering accidentally
GFX->clear( GFXClearZBuffer , ColorI(20,20,20), 1.0f, 0 );
GFX->setFrustum( frustum );
if(mLastCameraQuery.ortho)
{
mOrthoWidth = frustum.getWidth();
mOrthoHeight = frustum.getHeight();
}
// We're going to be displaying this render at size of this control in
// pixels - let the scene know so that it can calculate e.g. reflections
// correctly for that final display result.
gClientSceneGraph->setDisplayTargetResolution(getExtent());
// Set the GFX world matrix to the world-to-camera transform, but don't
// change the cameraMatrix in mLastCameraQuery. This is because
// mLastCameraQuery.cameraMatrix is supposed to contain the camera-to-world
// transform. In-place invert would save a copy but mess up any GUIs that
// depend on that value.
MatrixF worldToCamera = mLastCameraQuery.cameraMatrix;
worldToCamera.inverse();
GFX->setWorldMatrix( worldToCamera );
mSaveProjection = GFX->getProjectionMatrix();
mSaveModelview = GFX->getWorldMatrix();
mSaveViewport = updateRect;
mSaveWorldToScreenScale = GFX->getWorldToScreenScale();
mSaveFrustum = GFX->getFrustum();
mSaveFrustum.setTransform( mLastCameraQuery.cameraMatrix );
// Set the default non-clip projection as some
// objects depend on this even in non-reflect cases.
gClientSceneGraph->setNonClipProjection( mSaveProjection );
// Give the post effect manager the worldToCamera, and cameraToScreen matrices
PFXMGR->setFrameMatrices( mSaveModelview, mSaveProjection );
renderWorld(updateRect);
DebugDrawer::get()->render();
// Restore the previous matrix state before
// we begin rendering the child controls.
saver.restore();
// Restore the render style and any stereo parameters
GFX->setCurrentRenderStyle(prevRenderStyle);
GFX->setCurrentProjectionOffset(prevProjectionOffset);
GFX->setStereoEyeOffset(prevEyeOffset);
// Allow subclasses to render 2D elements.
GFX->setClipRect(updateRect);
renderGui( offset, updateRect );
renderChildControls(offset, updateRect);
smFrameCount++;
}
arma::mat coordMat(double a1,double a2,double a3,rotationOrder order){
arma::mat ret;
ret.eye(4,4);
switch (order) {
case EULER_XYZ:
ret=rotMat(a1,0)*ret;
ret=rotMat(a2,1)*ret;
ret=rotMat(a3,2)*ret;
break;
case EULER_XZY:
ret=rotMat(a1,0)*ret;
ret=rotMat(a2,2)*ret;
ret=rotMat(a3,1)*ret;
break;
case EULER_YXZ:
ret=rotMat(a1,1)*ret;
ret=rotMat(a2,0)*ret;
ret=rotMat(a3,2)*ret;
break;
case EULER_YZX:
ret=rotMat(a1,1)*ret;
ret=rotMat(a2,2)*ret;
ret=rotMat(a3,0)*ret;
break;
case EULER_ZXY:
ret=rotMat(a1,2)*ret;
ret=rotMat(a2,0)*ret;
ret=rotMat(a3,1)*ret;
break;
case EULER_ZYX:
ret=rotMat(a1,2)*ret;
ret=rotMat(a2,1)*ret;
ret=rotMat(a3,0)*ret;
break;
default:
break;
}
return ret;
}
bool GoBoardDetector::calculateCameraIntrinsix()
{
//cv::showAndSave("marker_board_pre", undistortImage);
std::vector<cv::Point3f> m_markerCorners3d;
cv::Mat test;
undistortImage.copyTo(test);
m_markerCorners3d.clear();
std::vector<cv::Point2f> imgPoints;
bool hasBoardMarkers = false;
for(size_t i=0; i<m_detectedMarkers.size(); i++)
{
Marker& m = m_detectedMarkers[i];
//m.draw(undistortImage,cv::Scalar(0,0,255),2);
for(size_t j=0; j<boardMarkerID.size(); j++)
{
//if the marker is one of the board marker ids
if(m.id == boardMarkerID[j])
{
bool CorrectMarker = false;
float botL_x;
float botL_y;
//bottom row
if(j>= botLeft && j<= botRight)
{
CorrectMarker = true;
hasBoardMarkers = true;
int factor = j- botLeft;
botL_x = gap*factor;
botL_y = 0;
imgPoints.push_back(m.points[3]);
imgPoints.push_back(m.points[0]);
imgPoints.push_back(m.points[1]);
imgPoints.push_back(m.points[2]);
m_markerCorners3d.push_back(cv::Point3f(botL_x,botL_y,0));
m_markerCorners3d.push_back(cv::Point3f(botL_x,botL_y+1,0));
m_markerCorners3d.push_back(cv::Point3f(botL_x+1,botL_y+1,0));
m_markerCorners3d.push_back(cv::Point3f(botL_x+1,botL_y,0));
}//top row
else if(j>=0 && j<=topRight)
{
CorrectMarker = true;
hasBoardMarkers = true;
int factor = j;
botL_x = gap*factor;
botL_y = (yMarkerNumber-1)*gap;
imgPoints.push_back(m.points[3]);
imgPoints.push_back(m.points[0]);
imgPoints.push_back(m.points[1]);
imgPoints.push_back(m.points[2]);
m_markerCorners3d.push_back(cv::Point3f(botL_x,botL_y,0));
m_markerCorners3d.push_back(cv::Point3f(botL_x,botL_y+1,0));
m_markerCorners3d.push_back(cv::Point3f(botL_x+1,botL_y+1,0));
m_markerCorners3d.push_back(cv::Point3f(botL_x+1,botL_y,0));
}//left row
else if((j-topRight)%2 == 1)
{
CorrectMarker = true;
hasBoardMarkers = true;
int factor = (botLeft-j)/2;
botL_x = 0;
botL_y = gap*factor;
imgPoints.push_back(m.points[3]);
imgPoints.push_back(m.points[0]);
imgPoints.push_back(m.points[1]);
imgPoints.push_back(m.points[2]);
m_markerCorners3d.push_back(cv::Point3f(botL_x,botL_y,0));
m_markerCorners3d.push_back(cv::Point3f(botL_x,botL_y+1,0));
m_markerCorners3d.push_back(cv::Point3f(botL_x+1,botL_y+1,0));
m_markerCorners3d.push_back(cv::Point3f(botL_x+1,botL_y,0));
}//right row
else if((j-topRight)%2 == 0)
{
CorrectMarker = true;
hasBoardMarkers = true;
int factor = (botLeft-(j-1))/2;
botL_x = (xMarkerNumber-1)*gap;
botL_y = gap*factor;
imgPoints.push_back(m.points[3]);
imgPoints.push_back(m.points[0]);
imgPoints.push_back(m.points[1]);
imgPoints.push_back(m.points[2]);
m_markerCorners3d.push_back(cv::Point3f(botL_x,botL_y,0));
m_markerCorners3d.push_back(cv::Point3f(botL_x,botL_y+1,0));
m_markerCorners3d.push_back(cv::Point3f(botL_x+1,botL_y+1,0));
m_markerCorners3d.push_back(cv::Point3f(botL_x+1,botL_y,0));
}
if(CorrectMarker){
if(showMarkers){
m.draw(undistortImage,cv::Scalar(0,0,255),2);
}
}
}
}
}
if(hasBoardMarkers == false){
GoBoardRaux = cv::Mat();
GoBoardTaux = cv::Mat();
return false;
}
cv::Mat transform_r;
BoardImagePoint.clear();
//two methods
if(m_PoseMethod == SOLVEPNP){
cv::Mat iprts;
cv::Mat raux, taux;
cv::solvePnP(m_markerCorners3d, imgPoints, camMatrix, distCoeff,transform_r,taux);
transform_r.convertTo(GoBoardRaux,CV_32F);
taux.convertTo(GoBoardTaux ,CV_32F);
cv::Rodrigues(transform_r, GoBoardRaux);
}else{
//test
cv::Mat m_markerCorners3d_RPP = cv::Mat::zeros(3, m_markerCorners3d.size(), CV_64F); // 3D points, z is zero
cv::Mat imgPoints_RPP = cv::Mat::ones(3, m_markerCorners3d.size(), CV_64F); // 2D points, homogenous points
cv::Mat_<float> rotMat(3,3);
int iterations;
double obj_err;
double img_err;
//cv::Mat iprts;
std::vector<cv::Point2f> iprts;
cv::undistortPoints(cv::Mat(imgPoints),iprts,camMatrix, distCoeff);
for(size_t i=0; i<iprts.size();i++)
{
m_markerCorners3d_RPP.at<double>(0,i) = m_markerCorners3d[i].x;
m_markerCorners3d_RPP.at<double>(1,i) = m_markerCorners3d[i].y;
imgPoints_RPP.at<double>(0,i) = iprts[i].x;
imgPoints_RPP.at<double>(1,i) = iprts[i].y;
}
if(!RPP::Rpp(m_markerCorners3d_RPP, imgPoints_RPP, GoBoardRaux, GoBoardTaux, iterations, obj_err, img_err)) {
fprintf(stderr, "Error with RPP\n");
return 1;
}
cv::solvePnP(m_markerCorners3d, imgPoints, camMatrix, distCoeff,transform_r,GoBoardTaux);
cv::Rodrigues(GoBoardRaux, transform_r);
}
//project 3d points on the image plane
//std::cout<<transform_r<<std::endl;
cv::projectPoints(Board3DPoint, transform_r, GoBoardTaux, camMatrix, distCoeff, BoardImagePoint);
//cv::showAndSave("marker_board", undistortImage);
if(showMarkers){
for(size_t c=0;c<BoardImagePoint.size() ; c++){
//MyFilledCircle(undistortImage, m_detectedMarkers[i].points[c],cv::Scalar(0,255,0));
//if(c<4)
// cv::line(undistortImage, BoardImagePoint[c], BoardImagePoint[(c+1)%4], cv::Scalar(255,0,0), 2, CV_AA);
MyFilledCircle(undistortImage, BoardImagePoint[c], cv::Scalar(0,0,255));
}
}
fullBoardInScene = true;
for(int i=0; i<4;i++){
if(BoardImagePoint[i].x<0 || BoardImagePoint[i].x >frameWidth ||
BoardImagePoint[i].y<0 || BoardImagePoint[i].y >frameHeight)
fullBoardInScene = false;
}
//boardImagePoint[0-3] are the four corners
//if(BoardImagePoint[0].x
//cv::showAndSave("marker_board_with_board_pts", undistortImage);
if(showMarkers){
cv::imshow("markers",undistortImage);
cv::waitKey(1);
}
return true;
}
Ray SceneVPMObject::scatter(const Ray& inRay, const bool fixIsLight, const bool russian) const
{
Ray outRay;
outRay.directionSampleType = Ray::RANDOM;
HomoMediaDistSampler logDistSampler(dt);
float sampDist = logDistSampler.sampleDist();
bool go_in_vol = (inRay.intersectObject == this) && (inRay.insideObject != this);
bool be_in_vol = (inRay.insideObject == this);
bool out_of_vol = (sampDist >= inRay.intersectDist); // hit a surface
// go in volume
if(go_in_vol){
vec3f position = inRay.origin + inRay.direction*inRay.intersectDist;
LocalFrame lf = inRay.intersectObject->getAutoGenWorldLocalFrame(inRay.intersectObjectTriangleID, position, flatNormals);
vec3f normal = lf.n;
outRay.origin = position;
outRay.direction = inRay.direction;
vec3f reflDir = -normal.dot(inRay.direction)*normal*2 + inRay.direction;
reflDir.normalize();
float theta = acos(inRay.direction.dot(normal));
SceneObject* currentInsideObject = inRay.insideObject;
SceneObject* outSideObject = (SceneObject*)this;
float current_n = currentInsideObject ? currentInsideObject->getRefrCoeff() : 1;
float next_n = outSideObject ? outSideObject->getRefrCoeff() : 1;
float sin_phi = current_n / next_n * sin(theta);
outRay.intersectObject = NULL;
outRay.color = vec3f(1, 1, 1);
outRay.directionProb = 1;
outRay.contactObject = (SceneObject*)this;
outRay.contactObjectTriangleID = inRay.intersectObjectTriangleID;
if(sin_phi >= 1){ // no total internal reflection
outRay.direction = vec3f(0.f);
outRay.contactObject = NULL;
outRay.contactObjectTriangleID = -1;
outRay.directionSampleType = Ray::DEFINITE;
return outRay;
// outRay.direction = reflDir;
// outRay.insideObject = inRay.insideObject;
// outRay.directionProb = 1;
// outRay.directionSampleType = Ray::DEFINITE;
// outRay.photonType = Ray::NOUSE;
// outRay.color /= outRay.getCosineTerm();
//if (abs(outRay.getCosineTerm() - 1.f) > 1e-6f)
// printf("exception1: %.6f\n" , outRay.getCosineTerm());
}
else{
float phi = asin(sin_phi);
if(theta > M_PI/2)
phi = M_PI - phi;
vec3f axis = normal.cross(inRay.direction);
axis.normalize();
outRay.direction = vec3f(rotMat(axis, phi) * vec4<float>(normal, 0));
outRay.direction.normalize();
float cos_theta = abs(cos(theta));
float cos_phi = abs(cos(phi));
float esr = powf(abs(current_n*cos_theta-next_n*cos_phi)/(current_n*cos_theta+next_n*cos_phi),2);
float epr = powf(abs(next_n*cos_theta-current_n*cos_phi)/(next_n*cos_theta+current_n*cos_phi),2);
float er = (esr+epr)*0.5f;
float p = clampf(er , 0.f , 1.f);
if(RandGenerator::genFloat() < p)
{
outRay.direction = reflDir;
outRay.color *= er / outRay.getCosineTerm();
outRay.directionProb = p;
outRay.insideObject = inRay.insideObject;
outRay.directionSampleType = Ray::DEFINITE;
outRay.photonType = Ray::NOUSE;
}
else
{
if (!fixIsLight) outRay.color *= (current_n * current_n) / (next_n * next_n);
//printf("go in: %.6f/%.6f\n" , current_n , next_n);
outRay.color *= (1-er) / outRay.getCosineTerm();
outRay.directionProb = 1-p;
outRay.contactObject = outRay.insideObject = (SceneObject*)this;
outRay.directionSampleType = Ray::DEFINITE;
outRay.photonType = Ray::HITVOL;
}
outRay.direction.normalize();
}
return outRay;
}
// in volume
if(be_in_vol && !out_of_vol){
HGPhaseSampler hgPhaseSampler(g);
//IsotropicPhaseSampler sp;
LocalFrame lf; lf.buildFromNormal(inRay.direction);
outRay.origin = inRay.origin + inRay.direction * sampDist;
outRay.direction = hgPhaseSampler.genSample(lf);
outRay.color = bsdf->evaluate(lf, inRay.direction, outRay.direction);
outRay.insideObject = (SceneObject*)this;
//outRay.contactObject = NULL;
outRay.contactObjectTriangleID = inRay.intersectObjectTriangleID;
//outRay.contactObjectTriangleID = -1;
//outRay.directionProb = 1;
//outRay.color = vec3f(1, 1, 1);
float albedo = y(ds) / y(dt);
float rander = RandGenerator::genFloat();
//outRay.originSampleType = Ray::RANDOM;
if(rander < albedo || (!russian)){
outRay.contactObject = NULL;
float oPdfW = hgPhaseSampler.getProbDensity(lf, outRay.direction);//sp.getProbDensity(lf, outRay.direction);//
if (russian)
outRay.directionProb = albedo * oPdfW;
else
outRay.directionProb = oPdfW;
outRay.originProb = p_medium(sampDist);
outRay.directionSampleType = Ray::RANDOM;
outRay.color *= ds;
outRay.photonType = Ray::INVOL;
}
else{
// terminate
outRay.direction = vec3f(0, 0, 0);
outRay.color = vec3f(0, 0, 0);
outRay.directionProb = 1;
outRay.originProb = p_medium(sampDist);
outRay.insideObject = (SceneObject*)this; // FIXED
//outRay.insideObject = NULL;
outRay.contactObject = NULL;
outRay.directionSampleType = Ray::RANDOM;
outRay.photonType = Ray::INVOL;
//outRay.photonType = Ray::NOUSE;
}
return outRay;
}
// go out of volume
if(be_in_vol && out_of_vol){
outRay = inRay;
outRay.direction = inRay.direction;
outRay.origin = inRay.origin + inRay.intersectDist * inRay.direction;
outRay.contactObject = inRay.intersectObject;
outRay.contactObjectTriangleID = inRay.intersectObjectTriangleID;
outRay.insideObject = (SceneObject*)this;
outRay.directionProb = 1;
outRay.color = vec3f(1,1,1);
bool going_out = (inRay.intersectObject == this);
if(going_out){
LocalFrame lf = inRay.intersectObject->getAutoGenWorldLocalFrame(inRay.intersectObjectTriangleID, outRay.origin, flatNormals);
vec3f normal = lf.n;
vec3f reflDir = -normal.dot(inRay.direction)*normal*2 + inRay.direction;
reflDir.normalize();
float theta = acos(inRay.direction.dot(normal));
SceneObject* currentInsideObject = (SceneObject*)this;
SceneObject* outSideObject = scene->findInsideObject(outRay, (SceneObject*)this);
float current_n = currentInsideObject ? currentInsideObject->getRefrCoeff() : 1;
float next_n = outSideObject ? outSideObject->getRefrCoeff() : 1;
float sin_phi = current_n / next_n * sin(theta);
outRay.intersectObject = NULL;
if(sin_phi >= 1){ // no total internal reflection
outRay.direction = vec3f(0.f);
outRay.contactObject = NULL;
outRay.contactObjectTriangleID = -1;
outRay.directionSampleType = Ray::DEFINITE;
return outRay;
// outRay.direction = reflDir;
// outRay.insideObject = inRay.insideObject;
// outRay.contactObject = (SceneObject*)this;
// outRay.originProb = P_surface(inRay.intersectDist);
// outRay.photonType = Ray::NOUSE;
// outRay.directionSampleType = Ray::DEFINITE;
// outRay.color /= outRay.getCosineTerm();
//if (abs(outRay.getCosineTerm() - 1.f) > 1e-6f)
// printf("exception2: %.6f\n" , outRay.getCosineTerm());
}
else{
float phi = asin(sin_phi);
if(theta > M_PI/2)
phi = M_PI - phi;
vec3f axis = normal.cross(inRay.direction);
axis.normalize();
outRay.direction = vec3f(rotMat(axis, phi) * vec4<float>(normal, 0));
outRay.direction.normalize();
float cos_theta = abs(cos(theta));
float cos_phi = abs(cos(phi));
float esr = powf(abs(current_n*cos_theta-next_n*cos_phi)/(current_n*cos_theta+next_n*cos_phi),2);
float epr = powf(abs(next_n*cos_theta-current_n*cos_phi)/(next_n*cos_theta+current_n*cos_phi),2);
float er = (esr+epr)/2.f;
float p = clampf(er , 0.f , 1.f);
if(RandGenerator::genFloat() < p)
{
outRay.direction = reflDir;
outRay.color *= er / outRay.getCosineTerm();
outRay.directionProb = p;
outRay.originProb = P_surface(inRay.intersectDist);
outRay.insideObject = inRay.insideObject;
outRay.directionSampleType = Ray::DEFINITE;
outRay.photonType = Ray::NOUSE;
}
else
{
if (!fixIsLight) outRay.color *= (current_n * current_n) / (next_n * next_n);
//printf("go out: %.6f/%.6f\n" , current_n , next_n);
outRay.color *= (1-er) / outRay.getCosineTerm();
outRay.directionProb = (1-p);
outRay.originProb = P_surface(inRay.intersectDist);
outRay.insideObject = outSideObject;
outRay.directionSampleType = Ray::DEFINITE;
outRay.photonType = Ray::NOUSE;
}
outRay.direction.normalize();
}
}
else{
//std::cout << "in vol hit surface " << std::endl;
outRay.contactObject = NULL;
outRay.intersectDist = 0;
//------ FIX ME -------
if (inRay.intersectObject != NULL)
outRay = inRay.intersectObject->scatter(outRay , fixIsLight , russian);
else
printf("error VPM object scattering\n");
//---------------------
outRay.originProb *= P_surface(inRay.intersectDist);
outRay.photonType = inRay.intersectObject->isVolumetric() ? Ray::NOUSE : Ray::OUTVOL;
}
return outRay;
}
printf("error at SceneVPMObject: should not reach here\n");
return outRay;
}
Quaternion &Quaternion::operator=(const Matrix3 &rotMat)
{
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
// article "Quaternion Calculus and Fast Animation".
float trace = rotMat(0, 0) + rotMat(1, 1) + rotMat(2, 2);
float root;
if (trace > 0.0)
{
// |w| > 1/2, may as well choose w > 1/2
root = Sqrt(trace + 1.0f); // 2w
w = 0.5f * root;
root = 0.5f / root; // 1/(4w)
x = (rotMat(2, 1) - rotMat(1, 2)) * root;
y = (rotMat(0, 2) - rotMat(2, 0)) * root;
z = (rotMat(1, 0) - rotMat(0, 1)) * root;
}
else
{
// |w| <= 1/2
static int next[3] = { 1, 2, 0 };
int i = 0;
if (rotMat(1, 1) > rotMat(0, 0))
{
i = 1;
}
if (rotMat(2, 2) > rotMat(i, i))
{
i = 2;
}
int j = next[i];
int k = next[j];
root = Sqrt(rotMat(i, i) - rotMat(j, j) - rotMat(k, k) + 1.0f);
float* quat[3] = { &x, &y, &z };
*quat[i] = 0.5f * root;
root = 0.5f / root;
w = (rotMat(k, j) - rotMat(j, k)) * root;
*quat[j] = (rotMat(j, i) + rotMat(i, j)) * root;
*quat[k] = (rotMat(k, i) + rotMat(i, k)) * root;
}
return *this;
}
