void MyPrimitive::GeneratePlane(float a_fSize, vector3 a_v3Color)
{
if (a_fSize < 0.01f)
a_fSize = 0.01f;
Release();
Init();
float fValue = 0.5f * a_fSize;
vector3 pointA(-fValue, -fValue, 0.0f); //0
vector3 pointB(fValue, -fValue, 0.0f); //1
vector3 pointC(fValue, fValue, 0.0f); //2
vector3 pointD(-fValue, fValue, 0.0f); //3
vector3 pointE(fValue, -fValue, -0.001f); //1
vector3 pointF(-fValue, -fValue, -0.001f); //0
vector3 pointG(fValue, fValue, -0.001f); //2
vector3 pointH(-fValue, fValue, -0.001f); //3
//F
AddQuad(pointA, pointB, pointD, pointC);
//Double sided
AddQuad(pointE, pointF, pointG, pointH);
CompileObject(a_v3Color);
}
void BasisSet::processDensity(BasisShell &shell)
{
BasisSet *set = shell.set;
unsigned int atomsSize = set->m_numAtoms;
unsigned int basisSize = set->m_symmetry.size();
unsigned int matrixSize = set->m_density.rows();
std::vector<int> &basis = set->m_symmetry;
vector<Vector3d> deltas;
vector<double> dr2;
deltas.reserve(atomsSize);
dr2.reserve(atomsSize);
// Calculate our position
Vector3d pos = shell.tCube->position(shell.pos) * ANGSTROM_TO_BOHR;
// Calculate the deltas for the position
for (unsigned int i = 0; i < atomsSize; ++i) {
deltas.push_back(pos - set->m_atomPos[i]);
dr2.push_back(deltas[i].squaredNorm());
}
// Calculate the basis set values at this point
MatrixXd values(matrixSize, 1);
for (unsigned int i = 0; i < basisSize; ++i) {
unsigned int cAtom = set->m_atomIndices[i];
switch(basis[i]) {
case S:
pointS(shell.set, dr2[cAtom], i, values);
break;
case P:
pointP(shell.set, deltas[cAtom], dr2[cAtom], i, values);
break;
case D:
pointD(shell.set, deltas[cAtom], dr2[cAtom], i, values);
break;
case D5:
pointD5(shell.set, deltas[cAtom], dr2[cAtom], i, values);
break;
default:
// Not handled - return a zero contribution
;
}
}
// Now calculate the value of the density at this point in space
double rho = 0.0;
for (unsigned int i = 0; i < matrixSize; ++i) {
// Calculate the off-diagonal parts of the matrix
for (unsigned int j = 0; j < i; ++j) {
rho += 2.0 * set->m_density.coeffRef(i, j)
* (values.coeffRef(i, 0) * values.coeffRef(j, 0));
}
// Now calculate the matrix diagonal
rho += set->m_density.coeffRef(i, i)
* (values.coeffRef(i, 0) * values.coeffRef(i, 0));
}
// Set the value
shell.tCube->setValue(shell.pos, rho);
}
/**
* recenters the points of a constant distance towards the center
*
* @param y bottom most point
* @param z left most point
* @param x right most point
* @param t top most point
* @return {@link vector<Ref<ResultPoint> >} describing the corners of the rectangular
* region. The first and last points are opposed on the diagonal, as
* are the second and third. The first point will be the topmost
* point and the last, the bottommost. The second point will be
* leftmost and the third, the rightmost
*/
vector<Ref<ResultPoint> > WhiteRectangleDetector::centerEdges(Ref<ResultPoint> y, Ref<ResultPoint> z,
Ref<ResultPoint> x, Ref<ResultPoint> t) {
//
// t t
// z x
// x OR z
// y y
//
float yi = y->getX();
float yj = y->getY();
float zi = z->getX();
float zj = z->getY();
float xi = x->getX();
float xj = x->getY();
float ti = t->getX();
float tj = t->getY();
std::vector<Ref<ResultPoint> > corners(4);
if (yi < (float)width_/2) {
Ref<ResultPoint> pointA(new ResultPoint(ti - CORR, tj + CORR));
Ref<ResultPoint> pointB(new ResultPoint(zi + CORR, zj + CORR));
Ref<ResultPoint> pointC(new ResultPoint(xi - CORR, xj - CORR));
Ref<ResultPoint> pointD(new ResultPoint(yi + CORR, yj - CORR));
corners[0].reset(pointA);
corners[1].reset(pointB);
corners[2].reset(pointC);
corners[3].reset(pointD);
} else {
Ref<ResultPoint> pointA(new ResultPoint(ti + CORR, tj + CORR));
Ref<ResultPoint> pointB(new ResultPoint(zi + CORR, zj - CORR));
Ref<ResultPoint> pointC(new ResultPoint(xi - CORR, xj + CORR));
Ref<ResultPoint> pointD(new ResultPoint(yi - CORR, yj - CORR));
corners[0].reset(pointA);
corners[1].reset(pointB);
corners[2].reset(pointC);
corners[3].reset(pointD);
}
return corners;
}
/// This is the stuff we actually use right now - porting to new data structure
void BasisSet::processPoint(BasisShell &shell)
{
BasisSet *set = shell.set;
unsigned int atomsSize = set->m_numAtoms;
unsigned int basisSize = set->m_symmetry.size();
std::vector<int> &basis = set->m_symmetry;
vector<Vector3d> deltas;
vector<double> dr2;
deltas.reserve(atomsSize);
dr2.reserve(atomsSize);
unsigned int indexMO = shell.state-1;
// Calculate our position
Vector3d pos = shell.tCube->position(shell.pos) * ANGSTROM_TO_BOHR;
// Calculate the deltas for the position
for (unsigned int i = 0; i < atomsSize; ++i) {
deltas.push_back(pos - set->m_atomPos[i]);
dr2.push_back(deltas[i].squaredNorm());
}
// Now calculate the value at this point in space
double tmp = 0.0;
for (unsigned int i = 0; i < basisSize; ++i) {
switch(basis[i]) {
case S:
tmp += pointS(shell.set, i,
dr2[set->m_atomIndices[i]], indexMO);
break;
case P:
tmp += pointP(shell.set, i, deltas[set->m_atomIndices[i]],
dr2[set->m_atomIndices[i]], indexMO);
break;
case D:
tmp += pointD(shell.set, i, deltas[set->m_atomIndices[i]],
dr2[set->m_atomIndices[i]], indexMO);
break;
case D5:
tmp += pointD5(shell.set, i, deltas[set->m_atomIndices[i]],
dr2[set->m_atomIndices[i]], indexMO);
break;
default:
// Not handled - return a zero contribution
;
}
}
// Set the value
shell.tCube->setValue(shell.pos, tmp);
}
inline vector<double> GaussianSetTools::calculateValues(const Vector3 &position) const
{
m_basis->initCalculation();
unsigned int atomsSize = static_cast<unsigned int>(m_molecule->atomCount());
size_t basisSize = m_basis->symmetry().size();
const std::vector<int> &basis = m_basis->symmetry();
const std::vector<unsigned int> &atomIndices = m_basis->atomIndices();
vector<Vector3> deltas;
vector<double> dr2;
deltas.reserve(atomsSize);
dr2.reserve(atomsSize);
// Calculate our position
Vector3 pos(position * ANGSTROM_TO_BOHR);
// Calculate the deltas for the position
for (size_t i = 0; i < atomsSize; ++i) {
deltas.push_back(pos - (m_molecule->atom(i).position3d() * ANGSTROM_TO_BOHR));
dr2.push_back(deltas[i].squaredNorm());
}
// Allocate space for the values to be calculated.
size_t matrixSize = m_basis->moMatrix().rows();
vector<double> values;
values.resize(matrixSize, 0.0);
// Now calculate the values at this point in space
for (unsigned int i = 0; i < basisSize; ++i) {
switch (basis[i]) {
case GaussianSet::S:
pointS(i, dr2[atomIndices[i]], values);
break;
case GaussianSet::P:
pointP(i, deltas[atomIndices[i]], dr2[atomIndices[i]], values);
break;
case GaussianSet::D:
pointD(i, deltas[atomIndices[i]], dr2[atomIndices[i]], values);
break;
case GaussianSet::D5:
pointD5(i, deltas[atomIndices[i]], dr2[atomIndices[i]], values);
break;
default:
// Not handled - return a zero contribution
;
}
}
return values;
}
void DepthRegistrationCPU::projectDepth(const cv::Mat &scaled, cv::Mat ®istered) const
{
registered = cv::Mat::zeros(sizeRegistered, CV_16U);
#pragma omp parallel for
for(size_t r = 0; r < (size_t)sizeRegistered.height; ++r)
{
const uint16_t *itD = scaled.ptr<uint16_t>(r);
const double y = lookupY.at<double>(0, r);
const double *itX = lookupX.ptr<double>();
for(size_t c = 0; c < (size_t)sizeRegistered.width; ++c, ++itD, ++itX)
{
const double depthValue = *itD / 1000.0;
if(depthValue < zNear || depthValue > zFar)
{
continue;
}
Eigen::Vector4d pointD(*itX * depthValue, y * depthValue, depthValue, 1);
Eigen::Vector4d pointP = proj * pointD;
const double z = pointP[2];
const double invZ = 1 / z;
const int xP = (fx * pointP[0]) * invZ + cx;
const int yP = (fy * pointP[1]) * invZ + cy;
if(xP >= 0 && xP < sizeRegistered.width && yP >= 0 && yP < sizeRegistered.height)
{
const uint16_t z16 = z * 1000;
uint16_t &zReg = registered.at<uint16_t>(yP, xP);
if(zReg == 0 || z16 < zReg)
{
zReg = z16;
}
}
}
}
}
void DepthRegistrationCPU::registerDepth(const cv::Mat &depth, cv::Mat ®istered)
{
registered = cv::Mat::zeros(sizeDepth, CV_16U);
Eigen::Matrix4d proj;
proj(0, 0) = rotation.at<double>(0, 0);
proj(0, 1) = rotation.at<double>(0, 1);
proj(0, 2) = rotation.at<double>(0, 2);
proj(0, 3) = translation.at<double>(0, 0);
proj(1, 0) = rotation.at<double>(1, 0);
proj(1, 1) = rotation.at<double>(1, 1);
proj(1, 2) = rotation.at<double>(1, 2);
proj(1, 3) = translation.at<double>(1, 0);
proj(2, 0) = rotation.at<double>(2, 0);
proj(2, 1) = rotation.at<double>(2, 1);
proj(2, 2) = rotation.at<double>(2, 2);
proj(2, 3) = translation.at<double>(2, 0);
proj(3, 0) = 0;
proj(3, 1) = 0;
proj(3, 2) = 0;
proj(3, 3) = 1;
const double fx = cameraMatrixDepth.at<double>(0, 0);
const double fy = cameraMatrixDepth.at<double>(1, 1);
const double cx = cameraMatrixDepth.at<double>(0, 2) + 0.5;
const double cy = cameraMatrixDepth.at<double>(1, 2) + 0.5;
#pragma omp parallel for
for(size_t r = 0; r < (size_t)sizeDepth.height; ++r)
{
const uint16_t *itD = depth.ptr<uint16_t>(r);
const double y = lookupY.at<double>(0, r);
const double *itX = lookupX.ptr<double>();
for(size_t c = 0; c < (size_t)sizeDepth.width; ++c, ++itD, ++itX)
{
const double depthValue = *itD / 1000.0;
if(depthValue < zNear || depthValue > zFar)
{
continue;
}
Eigen::Vector4d pointD(*itX * depthValue, y * depthValue, depthValue, 1);
Eigen::Vector4d pointP = proj * pointD;
const double z = pointP[2];
const double invZ = 1 / z;
const int xP = (fx * pointP[0]) * invZ + cx;
const int yP = (fy * pointP[1]) * invZ + cy;
if(xP >= 0 && xP < sizeDepth.width && yP >= 0 && yP < sizeDepth.height)
{
const uint16_t z16 = z * 1000;
uint16_t &zReg = registered.at<uint16_t>(yP, xP);
if(zReg == 0 || z16 < zReg)
{
zReg = z16;
}
}
}
}
}
