void ScatterSky::_generateSkyPoints()
{
U32 rings=60, segments=20;//rings=160, segments=20;
Point3F tmpPoint( 0, 0, 0 );
// Establish constants used in sphere generation.
F32 deltaRingAngle = ( M_PI_F / (F32)(rings * 2) );
F32 deltaSegAngle = ( 2.0f * M_PI_F / (F32)segments );
// Generate the group of rings for the sphere.
for( int ring = 0; ring < 2; ring++ )
{
F32 r0 = mSin( ring * deltaRingAngle );
F32 y0 = mCos( ring * deltaRingAngle );
// Generate the group of segments for the current ring.
for( int seg = 0; seg < segments + 1 ; seg++ )
{
F32 x0 = r0 * sinf( seg * deltaSegAngle );
F32 z0 = r0 * cosf( seg * deltaSegAngle );
tmpPoint.set( x0, z0, y0 );
tmpPoint.normalizeSafe();
tmpPoint.x *= smEarthRadius + smAtmosphereRadius;
tmpPoint.y *= smEarthRadius + smAtmosphereRadius;
tmpPoint.z *= smEarthRadius + smAtmosphereRadius;
tmpPoint.z -= smEarthRadius;
if ( ring == 1 )
mSkyPoints.push_back( tmpPoint );
}
}
}
//===================================
// find the shortest distance from a
// given node in the list until its end
// used to find the distnace to the new
// number on the screen
//===================================
Point NumberList::shortestDistanceToNewNumber(Point curPoint)const{
Point p(0, 0), tmpPoint(0, 0);
unsigned int distance = 999;
NumberListNode *pTemp;
for (pTemp = getTailNumber(); pTemp != NULL; pTemp = pTemp->next){
tmpPoint = pTemp->data.getNumberPosition();
if (curPoint.pointDistance(tmpPoint) < distance){
distance = curPoint.pointDistance(tmpPoint);
p = tmpPoint;
}
}
return p;
}
//===================================
// Function to find the closest number on the screen
//===================================
Point NumberList::findClosestNumber(Point curPoint) const{
Point p(0,0),tmpPoint(0,0);
unsigned int distance=999;
if (!isEmpty()){ // if the list is not empty
for (NumberListNode* pNode = head; pNode != NULL; pNode = pNode->next){
tmpPoint = pNode->data.getNumberPosition();
if (curPoint.pointDistance(tmpPoint) < distance){ // point that is closer
distance = curPoint.pointDistance(tmpPoint);
p = tmpPoint;
}
}
}
return p;
}
//convert a point from camera to world space
void Reconstructor::cam2WorldSpace(VirtualCamera cam, cv::Point3f &p)
{
cv::Mat tmp(3,1,CV_32F);
cv::Mat tmpPoint(3,1,CV_32F);
tmpPoint.at<float>(0) = p.x;
tmpPoint.at<float>(1) = p.y;
tmpPoint.at<float>(2) = p.z;
tmp = -cam.rotationMatrix.t() * cam.translationVector ;
tmpPoint = cam.rotationMatrix.t() * tmpPoint;
p.x = tmp.at<float>(0) + tmpPoint.at<float>(0);
p.y = tmp.at<float>(1) + tmpPoint.at<float>(1);
p.z = tmp.at<float>(2) + tmpPoint.at<float>(2);
}
/*
======================================================================
Evaluate()
====================================================================== */
void hotdm::Evaluate( lwpp::DisplacementAccess &da )
{
// Get the position of the point
lwpp::Point3d point(da.getSource());
// do the waves
if (interpolation)
{
_ocean_context->eval2_xz( (float) point.x, (float) point.z);
} else
{
_ocean_context->eval_xz( (float) point.x, (float) point.z);
}
lwpp::Point3d tmpPoint(point);
if (doChop)
{
tmpPoint.x += _ocean_context->disp[0];
tmpPoint.y += _ocean_context->disp[1];
tmpPoint.z += _ocean_context->disp[2];
}
else
{
tmpPoint.y += _ocean_context->disp[1];
}
// The normals are not affected on a displacement map.
/* This DOESNT work for displacement nodes, But left the code to see how other controls can be done */
/*if (normals)
{
lwpp::Vector3d pnormal( _ocean_context->normal[0],
_ocean_context->normal[1],
_ocean_context->normal[2]);
pnormal.Normalize();
da.setAvgNormal(pnormal);
}*/
// Displace the point
da.setSource(tmpPoint);
}
/**
* @brief Compute the disparity map based on the Euclidean distance of corresponding points.
* @param[in] matchMap A matrix of points, the same size as the left channel. Each cell of this
* matrix stores the location of the corresponding point in the right image.
* @param[out] dispMat The disparity map.
* @sa quantizeDisparity
* @sa getDisparity
*/
void computeDisparity(const cv::Mat_<cv::Point2i> &matchMap,
cv::Mat_<float> &dispMat)
{
for(int row=0; row< height; row++)
{
for(int col=0; col<width; col++)
{
cv::Point2d tmpPoint(col, row);
if (matchMap.at<cv::Point2i>(tmpPoint) == NO_MATCH)
{
dispMat.at<float>(tmpPoint) = 200;
continue;
}
//if a match is found, compute the difference in location of the match and current
//pixel.
int dx = col-matchMap.at<cv::Point2i>(tmpPoint).x;
int dy = row-matchMap.at<cv::Point2i>(tmpPoint).y;
//calculate disparity of current pixel.
dispMat.at<float>(tmpPoint) = sqrt(float(dx*dx+dy*dy));
}
}
}
void ColoredPointCloud3DTest::testMassiveData() {
int maxCount = 307200; //VGA resolution for realistic test
int maxCaptures = 2;
PointCloud3D* sensorData;
for (int captures = 0; captures < maxCaptures; ++captures) {
sensorData = new PointCloud3D();
(*sensorData->getPointCloud()).resize(maxCount);
for (int i = 0; i < static_cast<int>(sensorData->getPointCloud()->size()); i++) {
// outputPointCloud->addPoint(brics_3d::Point3D (inputPointCloud->points[i].x, inputPointCloud->points[i].y, inputPointCloud->points[i].z));
(*sensorData->getPointCloud()).replace(i, new ColoredPoint3D(new Point3D()));
(*sensorData->getPointCloud())[i].setX(1);
(*sensorData->getPointCloud())[i].setY(2);
(*sensorData->getPointCloud())[i].setZ(3);
(*sensorData->getPointCloud())[i].asColoredPoint3D()->setR(128);
(*sensorData->getPointCloud())[i].asColoredPoint3D()->setG(255);
(*sensorData->getPointCloud())[i].asColoredPoint3D()->setB(128);
}
CPPUNIT_ASSERT_EQUAL(maxCount, static_cast<int>(sensorData->getPointCloud()->size()));
delete sensorData;
}
PointCloud3D* sensorData2;
for (int captures = 0; captures < maxCaptures; ++captures) {
sensorData2 = new PointCloud3D();
for (int i = 0; i < maxCount; i++) {
sensorData2->addPointPtr(new ColoredPoint3D(new Point3D(1,2,3), 128, 255, 128)); //this will create a memory leak
Point3D tmpPoint(1,2,3);
// sensorData2->addPoint(ColoredPoint3D(&tmpPoint, 128, 255, 128)); //this will not create a memory leak
}
CPPUNIT_ASSERT_EQUAL(maxCount, static_cast<int>(sensorData2->getPointCloud()->size()));
delete sensorData2;
}
}
/* ======================================================================
Evaluate()
====================================================================== */
void HOTNode2D::Evaluate(LWNodalAccess *na, NodeOutputID outID, NodeValue value)
{
int res = resolution;
inResolution->evaluate(na, &res);
// Avoid a crash if the users connects for example
// a int node with a negative value.
res = (res < 1) ? 1 : res;
int gridres = 1 << int(res);
double stepsize = globalScale->GetValuef();
inGlobalScale->evaluate(na, &stepsize);
stepsize /= (float) gridres;
double wHeight = waveHeight->GetValuef();
inWaveHeight->evaluate(na, &wHeight);
double sWave = shortestWave->GetValuef();
inShortestWave->evaluate(na, &sWave);
double chpnss = choppiness->GetValuef();
inChoppiness->evaluate(na, &chpnss);
double wSpeed = windSpeed->GetValuef();
inWindSpeed->evaluate(na, &wSpeed);
// Ocean.h expect the win direction in radians
double wDir = windDirection->GetValuef();
inWindDirection->evaluate(na, &wDir);
double wAlign = windAlign->GetValuef();
inWindAlign->evaluate(na, &wAlign);
double tmpDampRef = dampReflections->GetValuef();
inDampReflections->evaluate(na, &tmpDampRef);
float dRef = 1.0f - tmpDampRef;
double oDepth = oceanDepth->GetValuef();
inOceanDepth->evaluate(na, &oDepth);
int tmpSeed = seed;
inSeed->evaluate(na, &tmpSeed);
// For some strange reason there is no need to lock on Image or Pixel
// Filters when this node is used inside on Dennis Image or Pixel Filter nodes.
// But if this node is used on the Nodal Surface editor it crashes unles the locks
// are in, because multiple threads want to delete the ocean several times.
thGroup.lockMutex(lwpp::MUTEX0);
// if we need to (re)initialize the ocean, do this
if (!_ocean || _ocean_needs_rebuild)
{
if (_ocean) delete _ocean;
if (_ocean_context) delete _ocean_context;
_ocean = new drw::Ocean( gridres, // M in Ocean.h
gridres, // N in Ocean.h
stepsize, // dx in Ocean.h
stepsize, // dz in Ocean.h
wSpeed, // V in Ocean.h
sWave, // l in Ocean.h
wHeight, // A in Ocean.h
wDir, // w in Ocean.h
dRef, // damp in Ocean.h
wAlign, // alignment in Ocean.h
oDepth, // depth in Ocean.h
tmpSeed); // seed in Ocean.h
_ocean_scale = _ocean->get_height_normalize_factor();
_ocean_context = _ocean->new_context(true,doChop,doNormals,doJacobian);
// sum up the waves at this timestep
_ocean->update( (float) newTime,
*_ocean_context,
true,
doChop,
doNormals,
doJacobian,
_ocean_scale * (float) wHeight,
chpnss);
_ocean_needs_rebuild = false;
}
// Do CatmullRom interpolation, or linear
if (doInterpolation)
{
_ocean_context->eval_ij( na->sx, na->sy);
}
lwpp::Point3d tmpPoint(0.0, 0.0, 0.0);
// Only do the chopinnes if is active
// and normals inactive (because we do not delete the Do normals connection if Do Chop is activatechoppines).
if (doChop)
{
// On the nodal version we DO NOT add the vector
// The node adds it for us
tmpPoint.x = _ocean_context->disp[0];
tmpPoint.y = _ocean_context->disp[1];
tmpPoint.z = _ocean_context->disp[2];
}
else
{
// On the nodal version we DO NOT add the vector
// The node adds it for us.
// x and y MUST be 0, or it will be like addd the point to itself,
// doubling the size of the ocean. Try it out commenting the .x .y lines.
tmpPoint.x = 0;
tmpPoint.y = _ocean_context->disp[1];
tmpPoint.z = 0;
}
if (outDisplacement->isID(outID))
{
setValue(value, tmpPoint.asLWVector());
}
// Only check the outNormal output if doNormals is active
// and chopinnes inactive (because we do not delete the connection if you change choppines.
if (doNormals)
{
// Careful with things like this: (outNormal->isID(outNormal))
// That will not work on a million years! :)
if (outNormal->isID(outID))
{
// Ocean.h store the normals calculated on _ocean_context->eval2_xz
// into the _ocean_context->normal
// A zero vector.
lwpp::Vector3d normal(0.0, 0.0, 0.0);
// Geometric normal in world coordinates. This is the raw polygonal normal at the spot, unperturbed by smoothing or bump mapping.
// For a different and slightly sharpen result use this one
// lwpp::Vector3d normal(na->gNorm);
lwpp::Vector3d onormal( _ocean_context->normal[0],
_ocean_context->normal[1],
_ocean_context->normal[2]);
onormal.Normalize();
normal += onormal;
// If we DO NOT add onormal to LW normal,
// the result is more sharpened, but looks that ignores the
// normals altered by the displacement. Check with someone! ;)
setValue(value, normal);
}
}
if (doJacobian)
{
if (doChop && outFoam)
{
if(outFoam->isID(outID) && doChop)
{
lwpp::Vector3d vfoam(0.0, 0.0,0.0);
// lwpp::Vector3d vjacobian( _ocean_context->Eminus[0], 0.0, _ocean_context->Eminus[1]);
double jm = (double) _ocean_context->Jminus;
if (jm < 0.0)
{
vfoam.x = -jm;
vfoam.y = -jm;
vfoam.z = -jm;
}
// Drew recommended do this on his email, but not sure it works better
// than simply left the output unmodified. In fact
// looks like the unmodified output works better when it's connected to a gradient.
// Or connect it to a Vector2 Scalar node and use Maximum , Minimun, X, Y, or Z as options.
// vfoam *= _ocean_scale;
setValue(value, vfoam.asLWVector());
}
}
if(outSpray->isID(outID))
{
lwpp::Vector3d vspray(0.0, 0.0,0.0);
// lwpp::Vector3d vjacobian( _ocean_context->Eminus[0], 0.0, _ocean_context->Eminus[1]);
double jm = (double) _ocean_context->Jminus;
// Posible calculation for spray particles
double jt = 1.0;
if (jm < jt)
{
vspray.x = -jm;
vspray.y = -jm;
vspray.z = -jm;
}
double jt_jm = jt - jm;
// The interpolated normal in world coordinates.
lwpp::Vector3d normal(na->wNorm0);
lwpp::Vector3d veminus( _ocean_context->Eminus[0], _ocean_context->Eminus[1], _ocean_context->Eminus[2]);
veminus *= jt_jm;
vspray = veminus + normal / sqrt ( 1.0 + (jt_jm * jt_jm));
setValue(value, vspray.asLWVector());
}
// Careful with things like this: (outJMinus->isID(outJMinus))
// That will not work on a million years! :)
if (outJMinus->isID(outID))
{
double jm = (double) _ocean_context->Jminus;
if (jm < 0.0)
{
jm = -jm;
}
// Drew recommended do this on his email, but not sure it works better
// than simply left the output unmodified. In fact
// looks like the unmodified output works better when it's connected to a gradient.
// Or connect it to a Vector2 Scalar node and use Maximum , Minimun, X, Y, or Z as options.
// jm *= _ocean_scale;
setValue(value, jm);
}
if (outJPlus->isID(outID))
{
double jp = (double) _ocean_context->Jplus;
setValue(value, jp);
}
if (EignMinus->isID(outID))
{
// lwpp::Vector3d veminus( _ocean_context->Eminus[0], 0.0, _ocean_context->Eminus[1]);
lwpp::Vector3d veminus( _ocean_context->Eminus[0], _ocean_context->Eminus[1], _ocean_context->Eminus[2]);
veminus.Normalize();
setValue(value, veminus.asLWVector());
}
if (EignPlus->isID(outID))
{
lwpp::Vector3d veplus( _ocean_context->Eplus[0], _ocean_context->Eplus[1], _ocean_context->Eplus[2]);
veplus.Normalize();
setValue(value, veplus.asLWVector());
}
} //end if (doJacobian)
thGroup.unlockMutex(lwpp::MUTEX0);
}
EXPORT_C TInt TEFparser::GetSectionData(TDesC& aScriptFilepath, TPtrC& aSectiontag, TDesC16 &aTocspTestFile, RFs& aFs)
{
TInt err = KErrNone;
TInt pos = 0;
RFile file;
// open the .ini file
if (BaflUtils::FolderExists(aFs, aScriptFilepath))
{
if (BaflUtils::FileExists( aFs, aScriptFilepath ))
{
file.Open(aFs, aScriptFilepath, EFileRead | EFileShareAny);
TFileText aLineReader;
TBuf<256> iLine;
TBuf<256> tempsectID;
// create the section name to search for
tempsectID.Copy(KOpenBrk);
tempsectID.Append(aSectiontag);
tempsectID.Append(KCloseBrk);
// read the ini file a line at a time until you find the the section name
aLineReader.Set(file);
TInt foundTag = -1;
while (err != KErrEof && foundTag != 0)
{
err = aLineReader.Read(iLine);
if (err != KErrEof)
foundTag = iLine.Find(tempsectID);
}
// create the next open bracket to search for
TBuf<2> tempopenBrk;
tempopenBrk.Copy(KOpenBrk);
RFile testfile;
err = KErrNone;
foundTag = -1;
// while not at the end of the file and not found the next open bracket
while (err != KErrEof && foundTag != 0)
{
// get the next line of the .ini file
err = aLineReader.Read(iLine);
if (err != KErrEof)
{
// if the line of the file doesn't contain an open bracket, we are still in the section body
foundTag = iLine.Find(tempopenBrk);
if (BaflUtils::FolderExists(aFs, aTocspTestFile) && foundTag != 0)
{
// open the test file we are going to write all our section info into
if (BaflUtils::FileExists( aFs, aTocspTestFile ))
{
testfile.Open(aFs, aTocspTestFile, EFileWrite|EFileShareAny);
testfile.Seek(ESeekEnd, pos);
}
else
{
User::LeaveIfError(testfile.Create(aFs, aTocspTestFile, EFileWrite|EFileShareAny));
testfile.Open(aFs, aTocspTestFile, EFileWrite|EFileShareAny);
}
// append to line of the file end of line characters
iLine.Append(_L("\r\n"));
// write line of the code out to the test file in UNICODE format
TPtrC8 tmpPoint((TText8*)iLine.Ptr(),iLine.Size());
testfile.Write(tmpPoint);
testfile.Flush();
}
testfile.Close();
}
}
}
}
return KErrNone;
}
void BoundingBox3DExtractor::computeOrientedBoundingBox(IPoint3DIterator::IPoint3DIteratorPtr inputPointCloud, IHomogeneousMatrix44* resultTransform, Vector3D& resultBoxDimensions) {
assert(resultTransform != 0);
lowerBound.setX(std::numeric_limits<brics_3d::Coordinate>::max());
lowerBound.setY(std::numeric_limits<brics_3d::Coordinate>::max());
lowerBound.setZ(std::numeric_limits<brics_3d::Coordinate>::max());
upperBound.setX(-std::numeric_limits<brics_3d::Coordinate>::max());
upperBound.setY(-std::numeric_limits<brics_3d::Coordinate>::max());
upperBound.setZ(-std::numeric_limits<brics_3d::Coordinate>::max());
Eigen::MatrixXd eigenvectors;
Eigen::VectorXd eigenvalues;
pcaExtractor.computePrincipleComponents(inputPointCloud, eigenvectors, eigenvalues); //actually we compute a centroid twice...
pcaExtractor.computeRotationMatrix(eigenvectors, eigenvalues, resultTransform);
/* get min/max for PCA rotated points */
HomogeneousMatrix44* inverseRotation = new HomogeneousMatrix44();
*inverseRotation = *(resultTransform);
inverseRotation->inverse();
for (inputPointCloud->begin(); !inputPointCloud->end(); inputPointCloud->next()) {
Point3D tmpPoint(inputPointCloud->getX(), inputPointCloud->getY(), inputPointCloud->getZ());
tmpPoint.homogeneousTransformation(inverseRotation); // move _all_ points to new frame
Point3D* currentPoint = &tmpPoint;
/* adjust lower bound if necessary */
if (currentPoint->getX() <= lowerBound.getX()) {
lowerBound.setX(currentPoint->getX());
}
if (currentPoint->getY() <= lowerBound.getY()) {
lowerBound.setY(currentPoint->getY());
}
if (currentPoint->getZ() <= lowerBound.getZ()) {
lowerBound.setZ(currentPoint->getZ());
}
/* adjust upper bound if necessary */
if ( currentPoint->getX() >= upperBound.getX()) {
upperBound.setX(currentPoint->getX());
}
if (currentPoint->getY() >= upperBound.getY()) {
upperBound.setY(currentPoint->getY());
}
if (currentPoint->getZ() >= upperBound.getZ()) {
upperBound.setZ(currentPoint->getZ());
}
}
delete inverseRotation;
/* get centroid */
Centroid3D centroidExtractor;
Eigen::Vector3d centroid;
centroid = centroidExtractor.computeCentroid(inputPointCloud);
/* as centroid as translation */
double* matrixData;
matrixData = resultTransform->setRawData();
//djd RC5 - don't use centroid as centre of bounding box if only returning dimensions - you will not be able to reconstruct it
matrixData[12] = centroid[0];
matrixData[13] = centroid[1];
matrixData[14] = centroid[2];
LOG(DEBUG) << "Estimated transform for oriented bounding box: "<< std::endl << *resultTransform;
resultBoxDimensions.setX(fabs(upperBound.getX() - lowerBound.getX()));
resultBoxDimensions.setY(fabs(upperBound.getY() - lowerBound.getY()));
resultBoxDimensions.setZ(fabs(upperBound.getZ() - lowerBound.getZ()));
}
