ossimGpt ossimThreeParamDatum::shiftToWgs84(const ossimGpt &aPt)const
{
if(ossim::almostEqual(param1(), 0.0)&&
ossim::almostEqual(param2(), 0.0)&&
ossim::almostEqual(param3(), 0.0))
{
return ossimGpt(aPt.latd(),
aPt.lond(),
aPt.latd(),
ossimGpt().datum());
}
ossimEcefPoint p1 = aPt;
ossimEcefPoint p2;
if(withinMolodenskyRange(aPt.latd()))
{
ossimWgs84Datum wgs84;
double latin, lonin, hgtin;
double latout, lonout, hgtout;
double da = wgs84.ellipsoid()->getA() - ellipsoid()->getA();
double df = wgs84.ellipsoid()->getFlattening() - ellipsoid()->getFlattening();
latin = aPt.latr();
lonin = aPt.lonr();
hgtin = aPt.height();
if(aPt.isHgtNan())
{
hgtin = 0.0;
}
molodenskyShift(ellipsoid()->getA(), da, ellipsoid()->getFlattening(), df, param1(), param2(), param3(),
latin, lonin, hgtin,
latout, lonout, hgtout);
ossimGpt g;
g.latr(latout);
g.lonr(lonout);
g.height(hgtout);
g.datum(this);
return g;
}
else
{
p2 = ossimEcefPoint(p1.x() + theParam1,
p1.y() + theParam2,
p1.z() + theParam3);
}
return ossimGpt(p2); // defaults to WGS84
}
std::shared_ptr<Ellipsoid> Ellipsoid::fromNSphere(Eigen::VectorXd ¢er, double radius) {
const int dim = center.size();
MatrixXd C = MatrixXd::Zero(dim, dim);
C.diagonal().setConstant(radius);
std::shared_ptr<Ellipsoid> ellipsoid(new Ellipsoid(C, center));
return ellipsoid;
}
int main() {
Eigen::MatrixXd A(4,3);
A << -1, 0, 0,
0, -1, 0,
0, 0, -1,
1, 1, 1;
Eigen::VectorXd b(4);
b << 0, 0, 0, 1;
iris::Polyhedron polyhedron(A, b);
iris::Ellipsoid ellipsoid(3);
iris_mosek::inner_ellipsoid(polyhedron, &ellipsoid);
Eigen::MatrixXd C_expected(3,3);
C_expected << 0.2523, -0.0740, -0.0740,
-0.0740, 0.2523, -0.0740,
-0.0740, -0.0740, 0.2523;
Eigen::VectorXd d_expected(3);
d_expected << 0.2732, 0.2732, 0.2732;
valuecheckMatrix(ellipsoid.getC(), C_expected, 1e-3);
valuecheckMatrix(ellipsoid.getD(), d_expected, 1e-3);
return 0;
}
TEST(EllipsoidalBoundary, outside) {
EllipsoidalBoundary ellipsoid(Vector3d(-5, 0, 0), Vector3d(5, 0, 0), 15);
Candidate c;
c.current.setPosition(Vector3d(0, 25, 0));
ellipsoid.process(&c);
EXPECT_FALSE(c.isActive());
EXPECT_TRUE(c.hasProperty("Rejected"));
}
rspfGpt rspfSevenParamDatum::shift(const rspfGpt &aPt)const
{
const rspfDatum* aDatum = aPt.datum();
if( (ellipsoid()->getA()== aPt.datum()->ellipsoid()->getA())&&
(ellipsoid()->getB()== aPt.datum()->ellipsoid()->getB()))
{
return rspfGpt(aPt.latd(), aPt.lond(), aPt.height(), this);
}
if(aDatum)
{
return shiftFromWgs84( aDatum->shiftToWgs84(aPt) );
}
return aPt;
}
TEST(EllipsoidalBoundary, limitStep) {
EllipsoidalBoundary ellipsoid(Vector3d(-5, 0, 0), Vector3d(5, 0, 0), 15);
ellipsoid.setLimitStep(true);
ellipsoid.setMargin(0.5);
Candidate c;
c.setNextStep(2);
c.current.setPosition(Vector3d(7, 0, 0));
ellipsoid.process(&c);
EXPECT_DOUBLE_EQ(c.getNextStep(), 1.5);
}
static LWError SuperQ_Build( sqData *tool, MeshEditOp *edit )
{
int ok;
switch ( tool->shape ) {
case 0: ok = ellipsoid( edit, tool ); break;
case 1: ok = toroid( edit, tool ); break;
default: break;
}
tool->update = LWT_TEST_NOTHING;
return ok ? NULL : "Failed";
}
rspfWgs84Datum::rspfWgs84Datum()
:rspfThreeParamDatum("WGE",
"World Geodetic System 1984",
rspfEllipsoidFactory::instance()->wgs84(),
0.0, 0.0, 0.0,
-M_PI/2.0, M_PI/2.0, -M_PI, M_PI,
0.0, 0.0, 0.0)
{
if(!ellipsoid())
{
//ERROR
}
}
BaseIF* makeGeometry(Box& a_domain,
RealVect& a_origin,
Real& a_dx)
{
RealVect center;
RealVect radii;
bool insideRegular;
bool vertical;
// parse input file
ParmParse pp;
Vector<int> n_cell(SpaceDim);
pp.getarr("n_cell",n_cell,0,SpaceDim);
CH_assert(n_cell.size() == SpaceDim);
IntVect lo = IntVect::Zero;
IntVect hi;
for (int ivec = 0; ivec < SpaceDim; ivec++)
{
if (n_cell[ivec] <= 0)
{
pout() << "Bogus number of cells input = " << n_cell[ivec];
exit(1);
}
hi[ivec] = n_cell[ivec] - 1;
}
a_domain.setSmall(lo);
a_domain.setBig(hi);
Vector<Real> prob_lo(SpaceDim,1.0);
Real prob_hi;
pp.getarr("prob_lo",prob_lo,0,SpaceDim);
pp.get("prob_hi",prob_hi);
a_dx = (prob_hi-prob_lo[0])/n_cell[0];
for (int idir = 0; idir < SpaceDim; idir++)
{
a_origin[idir] = prob_lo[idir];
}
// ParmParse doesn't get RealVects, so work-around with Vector<Real>
Vector<Real> vectorCenter;
pp.getarr("center",vectorCenter,0,SpaceDim);
for (int idir = 0; idir < SpaceDim; idir++)
{
center[idir] = vectorCenter[idir];
}
Vector<Real> vectorRadii;
pp.getarr("radii",vectorRadii,0,SpaceDim);
for (int idir = 0; idir < SpaceDim; idir++)
{
radii[idir] = vectorRadii[idir];
}
Real rate;
pp.get("rate",rate);
// Parm Parse doesn't get bools, so work-around with int
int intInsideRegular;
pp.get("insideRegular",intInsideRegular);
if (intInsideRegular != 0) insideRegular = true;
if (intInsideRegular == 0) insideRegular = false;
int intVertical;
pp.get("vertical",intVertical);
if (intVertical != 0) vertical = true;
if (intVertical == 0) vertical = false;
IntVect zero=IntVect::Zero;
bool inside = true;
EllipsoidIF ellipsoid(radii,center,inside);
HelixIF implicit(ellipsoid,rate,insideRegular,vertical);
RealVect vectDx = RealVect::Unit;
vectDx *= a_dx;
GeometryShop workshop(implicit,0,vectDx);
// This generates the new EBIS
EBIndexSpace* ebisPtr = Chombo_EBIS::instance();
ebisPtr->define(a_domain,a_origin,a_dx,workshop);
return implicit.newImplicitFunction();
}
DEMParticlePArray
DEMParticleCreator::updatePeriodicDEMParticles(const OrientedBox& periodicDomain,
DEMParticlePArray& particles)
{
auto e0 = periodicDomain.axis(0) * (periodicDomain.extent(0) * 2);
auto e1 = periodicDomain.axis(1) * (periodicDomain.extent(1) * 2);
auto e2 = periodicDomain.axis(2) * (periodicDomain.extent(2) * 2);
std::vector<Vec> vertices = periodicDomain.vertices();
constexpr std::array<std::array<int, 4>, 6> faceIndices = {{
{{0, 4, 7, 3}} // x-
, {{1, 2, 6, 5}} // x+
, {{0, 1, 5, 4}} // y-
, {{2, 3, 7, 6}} // y+
, {{0, 3, 2, 1}} // z-
, {{4, 5, 6, 7}} // z+
}};
std::vector<Face> faces;
for (const auto& indices : faceIndices) {
int i0 = indices[0]; int i1 = indices[1];
int i2 = indices[2]; int i3 = indices[3];
Face face(vertices[i0], vertices[i1], vertices[i2], vertices[i3]);
faces.push_back(face);
}
// Check intersections
DEMParticlePArray extraParticles;
for (const auto& particle : particles) {
if (particle->getType() != DEMParticle::DEMParticleType::FREE) {
continue;
}
auto position = particle->currentPosition();
auto axis_a = particle->currentAxisA();
auto axis_b = particle->currentAxisB();
auto axis_c = particle->currentAxisC();
auto radius_a = particle->radiusA();
auto radius_b = particle->radiusB();
auto radius_c = particle->radiusC();
auto id = particle->getId();
// Create an ellipsoid object for ellipsoid-face intersection tests
Ellipsoid ellipsoid(id, position, axis_a, axis_b, axis_c,
radius_a, radius_b, radius_c);
int faceID = 1;
for (const auto& face : faces) {
auto status = ellipsoid.intersects(face);
// std::cout << "Face = " << face << "\n";
// std::cout << "status = " << std::boolalpha << status.first
// << " face " << static_cast<int>(status.second.first)
// << " , " << status.second.second << "\n";
if (status.first) {
std::vector<Vec> translations;
switch (static_cast<Boundary::BoundaryID>(faceID)) {
case Boundary::BoundaryID::NONE:
break;
case Boundary::BoundaryID::XMINUS:
{
Vec shift = e0;
//std::cout << " Loc: x-: " << shift << "\n";
translations.push_back(shift);
addExtraTranslations(shift, face, status, translations);
break;
}
case Boundary::BoundaryID::XPLUS:
{
Vec shift = -e0;
//std::cout << " Loc: x-: " << shift << "\n";
translations.push_back(shift);
addExtraTranslations(shift, face, status, translations);
break;
}
case Boundary::BoundaryID::YMINUS:
{
Vec shift = e1;
//std::cout << " Loc: y-: " << shift << "\n";
translations.push_back(shift);
addExtraTranslations(shift, face, status, translations);
break;
}
case Boundary::BoundaryID::YPLUS:
{
Vec shift = -e1;
//std::cout << " Loc: y-: " << shift << "\n";
translations.push_back(shift);
addExtraTranslations(shift, face, status, translations);
break;
}
case Boundary::BoundaryID::ZMINUS:
{
Vec shift = e2;
//std::cout << " Loc: z-: " << shift << "\n";
translations.push_back(shift);
addExtraTranslations(shift, face, status, translations);
break;
}
case Boundary::BoundaryID::ZPLUS:
{
Vec shift = -e2;
//std::cout << " Loc: z-: " << shift << "\n";
translations.push_back(shift);
addExtraTranslations(shift, face, status, translations);
break;
}
break;
}
// Create copies
particle->setType(DEMParticle::DEMParticleType::BOUNDARY_PERIODIC);
for (const auto& translation : translations) {
DEMParticleP newParticle = std::make_shared<DEMParticle>(*particle);
newParticle->setCurrentPosition(particle->currentPosition() +
translation);
newParticle->setPreviousPosition(particle->previousPosition() +
translation);
newParticle->setId(particle->getId());
newParticle->computeAndSetGlobalCoef();
extraParticles.push_back(newParticle);
}
}
faceID += 1;
}
}
// Remove duplicates
removeDuplicates(extraParticles);
return extraParticles;
}
/**
* Non-obvious side effect: Converts particle type to BOUNDARY_PERIODIC if
* it is on the boundary
*/
DEMParticlePArray
DEMParticleCreator::generatePeriodicDEMParticles(DEMParticlePArray& particles,
Box& spatialDomain,
REAL marginFactor,
REAL faceShiftFactor)
{
// Find the largest radius ellipsoid in the input set and set
// the margin to be marginFactor times that value
auto minmaxIter = std::minmax_element(particles.begin(), particles.end(),
[](const DEMParticleP& p1, const DEMParticleP& p2){
auto max_p1 = std::max({p1->radiusA(), p1->radiusB(), p1->radiusC()});
auto max_p2 = std::max({p2->radiusA(), p2->radiusB(), p2->radiusC()});
return max_p1 < max_p2;
});
auto minRadius = std::min({(*minmaxIter.first)->radiusA(),
(*minmaxIter.first)->radiusB(),
(*minmaxIter.first)->radiusC()});
auto maxRadius = std::max({(*minmaxIter.second)->radiusA(),
(*minmaxIter.second)->radiusB(),
(*minmaxIter.second)->radiusC()});
auto boundaryMargin = marginFactor*maxRadius;
auto faceShift = faceShiftFactor*minRadius;
// std::cout << "min rad = " << minRadius << " max rad = " << maxRadius << "\n";
// std::cout << "Extra boundary margin = " << boundaryMargin
// << " face shift = " << faceShift << "\n";
// Create an oriented box from the spatial domain
// and set up the faces
Box shrunkDomain(
spatialDomain.minCorner() + Vec(faceShift, faceShift, faceShift),
spatialDomain.maxCorner() - Vec(faceShift, faceShift, faceShift));
OrientedBox box(shrunkDomain);
std::vector<Vec> vertices = box.vertices();
constexpr std::array<std::array<int, 4>, 3> faceIndices = {{
{{0, 4, 7, 3}} // x-
, {{0, 1, 5, 4}} // y-
, {{0, 3, 2, 1}} // z-
}};
std::vector<Face> faces;
for (const auto& indices : faceIndices) {
int i0 = indices[0]; int i1 = indices[1];
int i2 = indices[2]; int i3 = indices[3];
Vec v0 = vertices[i0]; Vec v1 = vertices[i1];
Vec v2 = vertices[i2]; Vec v3 = vertices[i3];
Face face(v0, v1, v2, v3);
if (!face.isValid()) {
std::cout << "**ERROR** The face " << face << " is invalid but"
<< " continuing anyway\n.";
}
faces.push_back(face);
}
// Compute starting ID for extra particles
auto maxIter = std::max_element(particles.begin(), particles.end(),
[](const DEMParticleP& p1, const DEMParticleP& p2){
return p1->getId() < p2->getId();
});
auto particleID = (*maxIter)->getId();
//auto particleID = particles.size();
// Check intersections
REAL widthX = shrunkDomain.dimX();
REAL widthY = shrunkDomain.dimY();
REAL widthZ = shrunkDomain.dimZ();
DEMParticlePArray extraParticles;
for (const auto& particle : particles) {
auto position = particle->currentPosition();
auto axis_a = particle->currentAxisA();
auto axis_b = particle->currentAxisB();
auto axis_c = particle->currentAxisC();
auto radius_a = particle->radiusA();
auto radius_b = particle->radiusB();
auto radius_c = particle->radiusC();
auto id = particle->getId();
// Create an ellipsoid object for ellipsoid-face intersection tests
// *TODO* Generalize to sphere and other particle shapes.
Ellipsoid ellipsoid(id, position, axis_a, axis_b, axis_c,
radius_a, radius_b, radius_c);
int faceID = 1;
for (const auto& face : faces) {
auto status = ellipsoid.intersects(face);
// std::cout << "Face = " << face << "\n";
// std::cout << "status = " << std::boolalpha << status.first
// << " face " << static_cast<int>(status.second.first)
// << " , " << status.second.second << "\n";
if (status.first) {
std::vector<Vec> translations;
switch (static_cast<Boundary::BoundaryID>(faceID)) {
case Boundary::BoundaryID::NONE:
break;
case Boundary::BoundaryID::XMINUS:
{
Vec shift(widthX + boundaryMargin, 0, 0);
//std::cout << " Loc: x-: " << shift << "\n";
translations.push_back(shift);
addExtraTranslations(shift, boundaryMargin, Vec(0, 1, 1),
widthX, widthY, widthZ,
face, status, translations);
break;
}
case Boundary::BoundaryID::XPLUS:
break;
case Boundary::BoundaryID::YMINUS:
{
Vec shift(0, widthY + boundaryMargin, 0);
//std::cout << " Loc: y-: " << shift << "\n";
translations.push_back(shift);
addExtraTranslations(shift, boundaryMargin, Vec(1, 0, 1),
widthX, widthY, widthZ,
face, status, translations);
break;
}
case Boundary::BoundaryID::YPLUS:
break;
case Boundary::BoundaryID::ZMINUS:
{
Vec shift(0, 0, widthZ + boundaryMargin);
//std::cout << " Loc: z-: " << shift << "\n";
translations.push_back(shift);
addExtraTranslations(shift, boundaryMargin, Vec(1, 1, 0),
widthX, widthY, widthZ,
face, status, translations);
break;
}
case Boundary::BoundaryID::ZPLUS:
break;
}
// Create copies
particle->setType(DEMParticle::DEMParticleType::BOUNDARY_PERIODIC);
for (const auto& translation : translations) {
DEMParticleP newParticle = std::make_shared<DEMParticle>(*particle);
newParticle->setCurrentPosition(particle->currentPosition() +
translation);
newParticle->setPreviousPosition(particle->previousPosition() +
translation);
newParticle->setId(++particleID);
newParticle->computeAndSetGlobalCoef();
extraParticles.push_back(newParticle);
}
}
faceID += 2;
}
}
// Remove duplicates
removeDuplicates(extraParticles);
// Update the spatial domain
Box expandedDomain(
shrunkDomain.minCorner(),
shrunkDomain.maxCorner() + Vec(boundaryMargin, boundaryMargin, boundaryMargin));
spatialDomain = expandedDomain;
return extraParticles;
}