ZString GetString(int indent, const Orientation& orient)
{
return
"Orientation("
+ GetString(indent, orient.GetUp()) + ", "
+ GetString(indent, orient.GetForward()) + ")";
}
void ManaZoneRenderer::updateCard(CardModel* model, Card* card, int pos, int size, int hovercard)
{
Orientation o;
o.pos = glm::vec3(mPos.x + mWidth - CONST_CARDSEPERATION_HORI*pos - CONST_CARDSEPERATION_HORI / 2, mPos.y, mPos.z + mHeight / 2);
if (card->mIsTapped)
o.dir = glm::vec3(1, 0, 0);
else
o.dir = glm::vec3(0, 0, -1);
if (hovercard == card->mUniqueId)
{
o.pos = glm::vec3(gHighlightX, gHighlightY, gHighlightZ);
o.dir = glm::vec3(0, 0, 1);
}
o.up = glm::vec3(0, 1, 0);
o.calculateQuat();
if (hovercard == card->mUniqueId)
{
model->setHoverMovement(o, 1000);
}
else
{
model->setMovement(o, 1000);
}
}
void CmodelIGC::Update(Time now)
{
// Model motion now handled by CclusterIGC
float dt = (now - m_lastUpdate);
m_lastUpdate = now;
if (m_visibleF)
{
//
// set the thing's rotation
//
if (m_rotation.angle() != 0.0f)
{
if (m_decalF) {
m_pThingSite->Spin(m_rotation.angle() * dt);
}
else
{
Orientation o (m_pHitTest->GetOrientation());
o.PostRotate(m_rotation.axis(), m_rotation.angle() * dt);
o.Renormalize();
m_pHitTest->SetOrientation(o);
m_pThingSite->SetOrientation(o);
}
}
}
}
bool PoolTeeBox::Load(File* pf)
{
// Get vertex.
if (!pf->GetFloat(&m_teeVertex.x) ||
!pf->GetFloat(&m_teeVertex.y) ||
!pf->GetFloat(&m_teeVertex.z))
{
pf->ReportError("Failed to load tee box vertex.");
return false;
}
m_bs.SetCentre(m_teeVertex);
m_bs.SetRadius(TEE_BOX_DECAL_SIZE);
SetShadowSize(TEE_BOX_DECAL_SIZE); // TODO TEMP TEST
CreateShadow();
Orientation o;
o.SetVertex(m_teeVertex);
SetOrientation(o);
TEE_BOX_DECAL_SIZE = Engine::Instance()->GetConfigFloat("golf_tee_size");
return true;
}
//Called to update the display.
//You should call glutSwapBuffers after all of your rendering to display what you rendered.
//If you need continuous updates of the screen, call glutPostRedisplay() at the end of the function.
void display()
{
g_orient.UpdateTime();
glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
glClearDepth(1.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glutil::MatrixStack currMatrix;
currMatrix.Translate(glm::vec3(0.0f, 0.0f, -200.0f));
currMatrix.ApplyMatrix(glm::mat4_cast(g_orient.GetOrient()));
glUseProgram(theProgram);
currMatrix.Scale(3.0, 3.0, 3.0);
currMatrix.RotateX(-90);
//Set the base color for this object.
glUniform4f(baseColorUnif, 1.0, 1.0, 1.0, 1.0);
glUniformMatrix4fv(modelToCameraMatrixUnif, 1, GL_FALSE, glm::value_ptr(currMatrix.Top()));
g_pShip->Render("tint");
glUseProgram(0);
glutSwapBuffers();
glutPostRedisplay();
}
Matrix3e fromev(const energy xx,const energy yy,const energy zz,const Orientation& o) const {
cout << "Converting " << o.ToString() << endl;
Matrix3 intMatrix=o.GetAsMatrix();
cout << "Rotation Matrix determinate:" << intMatrix.det().si << endl;;
const static energy zero=0.0*Joules;
Matrix3e in_eigen_frame(xx, zero,zero,
zero,yy ,zero,
zero,zero,zz );
//Undo the eigensystem decomposition
Matrix3e result=intMatrix*in_eigen_frame*intMatrix.Inverted();
return result;
}
bool ThirdPersonCameraBase::Collision(const Orientation& before, const Orientation& after)
{
const BoundingSphere& bs = *GetBoundingSphere();
BoundingSphere bsBefore(before.GetVertex(), bs.GetRadius());
BoundingSphere bsAfter(after.GetVertex(), bs.GetRadius());
const WallPoly* pWp = m_heightServer.Intersects(bsBefore, bsAfter);
if (pWp)
{
return true; // We do collide with something
}
return false; // No collision
}
TEST (Orientation, JSON) {
VLOG(1) << "===Orientation JSON Test===";
Orientation u { 20.0, -15.0, 60.0 };
Orientation v;
Value orient;
orient = u.pack();
VLOG_IF(orient.IsObject(), 2) << "Output JSON: " << orient;
v.parse(orient);
VLOG_IF(v.pack().IsObject(), 2) << "Parsed JSON: " << orient;
EXPECT_TRUE(toleranceEquals(u.pitch, v.pitch, TOL));
EXPECT_TRUE(toleranceEquals(u.roll, v.roll, TOL));
EXPECT_TRUE(toleranceEquals(u.heading, v.heading, TOL));
}
int main(int argc, char **argv)
{
QApplication app(argc, argv);
OrientationView view;
Orientation orientation;
orientation.start();
QObject::connect(&orientation, SIGNAL(orientation(qreal,qreal,qreal,qreal)),
&view, SLOT(orientation(qreal,qreal,qreal,qreal)));
view.show();
return app.exec();
}
Position Position::operator*(const Orientation& orientation) const
{
Position result(*this) ;
result.m_value = orientation.getQuaternion()*result.m_value ;
return result ;
}
Force Force::operator*(const Orientation& i_orientation) const
{
Force result(*this) ;
result.m_value = i_orientation.getQuaternion()* result.m_value ;
return result ;
}
int MovementComponent::turnTowards(float targetPosX, float targetPosY, float targetPosZ, float speed, bool putInQueue, const char* animName)
{
Vec3f target(targetPosX, targetPosY, targetPosZ);
//if we don't want a queue, we must erase all existent movement nodes
if(!putInQueue)
clear();
if(speed < 0)
speed = m_speed; //use agent default
//orient towards the target
Orientation* o = new Orientation( m_eID, target, speed, false );
o->setAnimation( (animName == 0) ? m_orientAnimName : animName );
m_movementNodes.push_back( o );
return o->getID();
}
int MovementComponent::rotate(float targetRotX, float targetRotY, float targetRotZ, float speed, bool putInQueue, const char* animName)
{
Vec3f target(targetRotX, targetRotY, targetRotZ);
//if we don't want a queue, we must erase all existent movement nodes
if(!putInQueue)
clear();
if(speed < 0)
speed = m_speed; //use agent default
//rotate until we reach the target rotation
Orientation* o = new Orientation( m_eID, target, speed, true );
o->setAnimation( (animName == 0) ? m_orientAnimName : animName );
m_movementNodes.push_back( o );
return o->getID();
}
void EngineStatePoolShowShot::Update()
{
if (m_time < m_maxTime)
{
// POOL_ONLINE:
// DON'T call EngineStatePoolBase::Update();
// If the timer expires, the shot will be started, but we DON'T want to update
// the shot in this state, for consistency with the other (user-controlled) client.
// If a moving ball is outside of the view frustum, pull the camera back.
((PoolCamera*)s_pCamera.GetPtr())->SetPoolPullBackRequired(s_movingBallNotInFrustum);
s_movingBallNotInFrustum = false;
GetCamera()->Update();
if (!IsBirdsEye())
{
// Update the camera so it is always above the ball
float camY = GetCamera()->GetOrientation()->GetY();
float ballY = GetBall()->GetOrientation()->GetY();
ballY += 1.0f; // TODO CONFIG
if (camY < ballY)
{
Orientation o = *(GetCamera()->GetOrientation());
o.SetY(ballY);
GetCamera()->SetOrientation(o);
}
}
// May call TimerExpired.....
EngineState::Update();
if (m_time < m_maxTime)
{
Assert(m_pLevel.GetPtr());
//// GetEngine()->GetDayNightSky()->Update();
m_pLevel->GetScene()->Update();
UpdateGameObjects();
}
}
else
{
// DON'T update game objects
EngineState::Update();
}
}
Orientation difference( Orientation other ) const{
auto a = this->normalized();
auto b = other.normalized();
return { int8_t(b.rotation-a.rotation)
, a.flip_ver != b.flip_ver
, a.flip_hor != b.flip_hor
};
}
void Menu::run()
{
Navigator<ContinuousRobotDriver> navigator;
Orientation* orientation = Orientation::getInstance();
gUserInterface.waitForHand();
startMelody();
enc_left_front_write(0);
enc_right_front_write(0);
enc_left_back_write(0);
enc_right_back_write(0);
orientation->resetHeading();
navigator.findBox(PersistantStorage::getTargetXLocation(),
PersistantStorage::getTargetYLocation());
searchFinishMelody();
navigator.findBox(0, 0);
stopMelody();
}
static SVGImageContext
OrientViewport(const SVGImageContext& aOldContext,
const Orientation& aOrientation)
{
CSSIntSize viewportSize(aOldContext.GetViewportSize());
if (aOrientation.SwapsWidthAndHeight()) {
swap(viewportSize.width, viewportSize.height);
}
return SVGImageContext(viewportSize,
aOldContext.GetPreserveAspectRatio());
}
TEST (Orientation, Creation) {
VLOG(1) << "===Orientation Creation Test===";
Orientation good { 20.0, -15.0, 60.0 };
Orientation bad;
Orientation bad_pitch { 20.0, NAN, 60.0 };
Orientation bad_roll { NAN, -15.0, 60.0 };
Orientation bad_heading { 20.0, -15.0, NAN };
EXPECT_TRUE(good.isValid());
EXPECT_FALSE(bad.isValid());
EXPECT_FALSE(bad_pitch.isValid());
bad_pitch.pitch = 20.0;
EXPECT_TRUE(bad_pitch.isValid());
EXPECT_FALSE(bad_roll.isValid());
bad_roll.roll = -15.0;
EXPECT_TRUE(bad_roll.isValid());
EXPECT_FALSE(bad_heading.isValid());
bad_heading.heading = 60.0;
EXPECT_TRUE(bad_heading.isValid());
}
/**
* Receive new IMU data, calculate the new orientation and publish data on the topic
*/
int SpatialDataHandler(CPhidgetSpatialHandle spatial, void *userptr, CPhidgetSpatial_SpatialEventDataHandle *data, int count)
{
sensor_msgs::Imu imu;
sensor_msgs::MagneticField mag;
imu.header.frame_id = "base_link";
imu.header.stamp = ros::Time::now();
mag.header.frame_id = "base_link";
mag.header.stamp = ros::Time::now();
for(int i = 0; i< count; i++)
{
if (data[i]->angularRate[0] != 0 || data[i]->angularRate[1] != 0 || data[i]->angularRate[2] != 0)
orientation_calculation.updateAngles((float*)&(data[i]->angularRate[0]), (float*)&(data[i]->acceleration), data[i]->timestamp.seconds + ((float)data[i]->timestamp.microseconds)/1000000);
// Save info into the message and publish
imu.orientation = tf::createQuaternionMsgFromRollPitchYaw(orientation_calculation.get_roll(),orientation_calculation.get_pitch(), orientation_calculation.get_yaw());
// Set up the angular velocity field. the data->angularRate is in deg/sec while we need the unit to be in rad/s
imu.angular_velocity.x = data[i]->angularRate[0] * (3.14 / 180);
imu.angular_velocity.y = data[i]->angularRate[1] * (3.14 / 180);
imu.angular_velocity.z = data[i]->angularRate[2] * (3.14 / 180);
// Set up the acceleration field. The data->acceleration is in g while we need the unit to be in m/s^2
imu.linear_acceleration.x = data[i]->acceleration[0] * 9.81;
imu.linear_acceleration.y = data[i]->acceleration[1] * 9.81;
imu.linear_acceleration.z = data[i]->acceleration[2] * 9.81;
// Set up the magnetic_field field. The data->magneticField is in Gauss while we need the unit to be in Tesla.
mag.magnetic_field.x = data[i]->magneticField[0] * 0.0001;
mag.magnetic_field.y = data[i]->magneticField[1] * 0.0001;
mag.magnetic_field.z = data[i]->magneticField[2] * 0.0001;
if(imu_pub)
imu_pub.publish(imu);
if(mag_pub)
mag_pub.publish(mag);
}
spatialError = 0;
return 0;
}
int MovementComponent::goTo(float targetX, float targetY, float targetZ, float speed, bool putInQueue, const char* orientAnimName, const char* walkAnimName)
{
Vec3f target(targetX, targetY, targetZ);
//if we don't want a queue, we must erase all existent movement nodes
if(!putInQueue)
clear();
if(speed < 0)
speed = m_speed; //use agent default
//orient towards the target
Orientation* o = new Orientation( m_eID, target, speed );
o->setAnimation( (orientAnimName == 0) ? m_orientAnimName : orientAnimName );
m_movementNodes.push_back( o );
//move to the target position
Locomotion* l = new Locomotion( m_eID, target, speed );
l->setAnimation( (walkAnimName == 0) ? m_walkAnimName : walkAnimName );
m_movementNodes.push_back( l );
return l->getID();
}
MGRadBndry::MGRadBndry(const BoxArray& _grids,
const int _ngroups,
const Geometry& _geom) :
NGBndry(_grids,_ngroups,_geom)
{
if (first)
init(_ngroups);
first = 0;
const BoxArray& grids = boxes();
const Box& domain = geom.Domain();
// const Real* dx = geom.CellSize();
// const Real* xlo = geom.ProbLo();
// It is desirable that the type array be set up after static init.
// This is part of the reason this step is not in NGBndry.
for (OrientationIter fi; fi; ++fi) {
Orientation face = fi();
if (bcflag[face] == 2) {
bctypearray[face].resize(grids.size());
for (FabSetIter bi(bndry[face]); bi.isValid(); ++bi) {
int igrid = bi.index();
if (domain[face] == boxes()[igrid][face] &&
!geom.isPeriodic(face.coordDir())) {
const Box& face_box = bndry[face][bi].box();
bctypearray[face].set(igrid, new BaseFab<int>(face_box));
// We don't care about the bndry values here, only the type array.
#if 0
FORT_RADBNDRY2(bndry[face][bi].dataPtr(), dimlist(face_box),
bctypearray[face][igrid].dataPtr(),
dimlist(domain), dx, xlo, time);
#endif
}
}
}
}
}
TEST (Orientation, Normalize) {
VLOG(1) << "===Orientation Normalize Test===";
Orientation x { 220.0, -185.0, -60.0 };
Orientation y { -220.0, 185.0, 460.0 };
VLOG(2) << "Raw x " << x.pack();
VLOG(2) << "Raw y " << y.pack();
EXPECT_TRUE(x.normalize());
EXPECT_TRUE(y.normalize());
VLOG(2) << "Normalized x " << x.pack();
VLOG(2) << "Normalized y " << y.pack();
EXPECT_TRUE(toleranceEquals(x.roll, -140.0, TOL));
EXPECT_TRUE(toleranceEquals(y.roll, 140.0, TOL));
EXPECT_TRUE(toleranceEquals(x.pitch, 175.0, TOL));
EXPECT_TRUE(toleranceEquals(y.pitch, -175.0, TOL));
EXPECT_TRUE(toleranceEquals(x.heading, 300.0, TOL));
EXPECT_TRUE(toleranceEquals(y.heading, 100.0, TOL));
}
void In(const proto::GameStateUpdate& gsu) {
this->state_id = gsu.state_id();
for (int e = 0; e < gsu.entity_size(); ++e) {
const proto::Entity& entity = gsu.entity(e);
eid entity_id = entity.id();
for (int i = 0; i < entity.components_size(); ++i) {
const proto::Component& comp = entity.components(i);
switch (comp.component_case()) {
case proto::Component::kPosition:
{
Position pos;
pos.In(comp);
this->positions[entity_id] = pos;
}
break;
case proto::Component::kOrientation:
{
Orientation orientation;
orientation.In(comp);
this->orientations[entity_id] = orientation;
}
break;
case proto::Component::kVelocity:
{
Velocity vel;
vel.In(comp);
this->velocities[entity_id] = vel;
}
break;
default:
// intentionally not handling other cases.
break;
}
}
}
}
//Called whenever a key on the keyboard was pressed.
//The key is given by the ''key'' parameter, which is in ASCII.
//It's often a good idea to have the escape key (ASCII value 27) call glutLeaveMainLoop() to
//exit the program.
void keyboard(unsigned char key, int x, int y)
{
switch (key)
{
case 27:
glutLeaveMainLoop();
return;
case 32:
{
bool bSlerp = g_orient.ToggleSlerp();
printf(bSlerp ? "Slerp\n" : "Lerp\n");
}
break;
}
for(int iOrient = 0; iOrient < ARRAY_COUNT(g_OrientKeys); iOrient++)
{
if(key == g_OrientKeys[iOrient])
ApplyOrientation(iOrient);
}
}
void PoolCue::Draw()
{
#ifdef CUE_DEBUG
glLineWidth(2.0f);
glBegin(GL_LINES);
glVertex3f(m_v1.x, m_v1.y, m_v1.z);
glVertex3f(m_v2.x, m_v2.y, m_v2.z);
glEnd();
return;
#endif
// Position cue - take into account
// - ball pos
// - Y-rot of player; i.e. the shot angle
// - cue elevation; i.e. masse
// - cue contact pos
// Find ball centre.
Orientation o = *(GetBall()->GetOrientation());
// Move the centre depending on draw/roll/english
// Find player shot direction.
float yr = Engine::Instance()->GetGameState()->GetCurrentPlayerInfo()->m_golfStroke.m_yRot;
float elev = Engine::Instance()->GetGameState()->GetCurrentPlayerInfo()->m_golfStroke.m_cueElevationDegs;
#ifdef CUE_DEBUG
std::cout << "POOL CUE: Elev: " << elev << " degs\n";
#endif
// If in birds eye mode, move the cue up a bit so it doesn't intersect
// the table.
if (EngineStatePoolBase::IsBirdsEye())
{
o.SetY(o.GetY() + 2.0f);
}
AmjuGL::PushMatrix();
o.SetZRot(elev);
o.SetYRot(yr + 90.0f);
o.Draw();
// Set contact pos
AmjuGL::Translate(0, m_y, m_x);
// Swing cue forward or back
AmjuGL::Translate(m_swingPos, 0, 0);
m_pSolid->Draw();
AmjuGL::PopMatrix();
}
void OnOrientationChanged( Orientation orientation )
{
unsigned int degrees = orientation.GetDegrees();
Rotate( static_cast< DeviceOrientation >( degrees ) );
}
int Orientation_Test01(const bool verbose) {
Teuchos::RCP<std::ostream> outStream;
Teuchos::oblackholestream bhs; // outputs nothing
if (verbose)
outStream = Teuchos::rcp(&std::cout, false);
else
outStream = Teuchos::rcp(&bhs, false);
Teuchos::oblackholestream oldFormatState;
oldFormatState.copyfmt(std::cout);
typedef typename
Kokkos::Impl::is_space<DeviceSpaceType>::host_mirror_space::execution_space HostSpaceType ;
*outStream << "DeviceSpace:: "; DeviceSpaceType::print_configuration(*outStream, false);
*outStream << "HostSpace:: "; HostSpaceType::print_configuration(*outStream, false);
*outStream << "\n";
*outStream
<< "===============================================================================\n"
<< "| |\n"
<< "| Unit Test (Orientation) |\n"
<< "| |\n"
<< "===============================================================================\n";
int errorFlag = 0;
try {
ordinal_type nthrow = 0, ncatch = 0;
{
Orientation ort;
INTREPID2_TEST_ERROR_EXPECTED( if (ort.isAlignedToReference()) \
throw std::logic_error("Default Orientation is not zero"); );
}
if (nthrow != ncatch) {
errorFlag++;
*outStream << std::setw(70) << "^^^^----FAILURE!" << "\n";
*outStream << "# of catch ("<< ncatch << ") is different from # of throw (" << nthrow << ")\n";
}
{
*outStream << "\n -- Testing Triangle \n\n";
const auto cellTopo = shards::CellTopology(shards::getCellTopologyData<shards::Triangle<3> >() );
const ordinal_type elemNodes[6][3] = { { 1, 2, 3 },
{ 1, 3, 2 },
{ 2, 1, 3 },
{ 2, 3, 1 },
{ 3, 1, 2 },
{ 3, 2, 1 } };
const ordinal_type refEdgeOrts[6][3] = { { 0, 0, 1 },
{ 0, 1, 1 },
{ 1, 0, 1 },
{ 0, 1, 0 },
{ 1, 0, 0 },
{ 1, 1, 0 } };
for (auto i=0;i<6;++i) {
// find orientation
const auto nodes = Kokkos::View<const ordinal_type[3],HostSpaceType>(elemNodes[i]);
const auto ort = Orientation::getOrientation(cellTopo, nodes);
// decode orientation
ordinal_type edgeOrt[3] = {};
for (auto edgeId=0;edgeId<3;++edgeId)
ort.getEdgeOrientation(edgeOrt, 3);
*outStream << " elemNodes = "
<< elemNodes[i][0] << " "
<< elemNodes[i][1] << " "
<< elemNodes[i][2] << " :: "
<< " computed edgeOrts = "
<< edgeOrt[0] << " "
<< edgeOrt[1] << " "
<< edgeOrt[2] << " :: "
<< " reference edgeOrts = "
<< refEdgeOrts[i][0] << " "
<< refEdgeOrts[i][1] << " "
<< refEdgeOrts[i][2] << " ::\n";
if (edgeOrt[0] != refEdgeOrts[i][0] ||
edgeOrt[1] != refEdgeOrts[i][1] ||
edgeOrt[2] != refEdgeOrts[i][2]) {
*outStream << " ^^^^^^^^^^^^^^^^ FAILURE\n";
++errorFlag;
}
}
}
{
*outStream << "\n -- Testing Quadrilateral \n\n";
const auto cellTopo = shards::CellTopology(shards::getCellTopologyData<shards::Quadrilateral<4> >() );
const ordinal_type elemNodes[24][4] = { { 1 , 2 , 3 , 4 },
{ 2 , 1 , 3 , 4 },
{ 1 , 3 , 2 , 4 },
{ 2 , 3 , 1 , 4 },
{ 3 , 1 , 2 , 4 },
{ 3 , 2 , 1 , 4 },
{ 1 , 2 , 4 , 3 },
{ 2 , 1 , 4 , 3 },
{ 1 , 3 , 4 , 2 },
{ 2 , 3 , 4 , 1 },
{ 3 , 1 , 4 , 2 },
{ 3 , 2 , 4 , 1 },
{ 1 , 4 , 2 , 3 },
{ 2 , 4 , 1 , 3 },
{ 1 , 4 , 3 , 2 },
{ 2 , 4 , 3 , 1 },
{ 3 , 4 , 1 , 2 },
{ 3 , 4 , 2 , 1 },
{ 4 , 1 , 2 , 3 },
{ 4 , 2 , 1 , 3 },
{ 4 , 1 , 3 , 2 },
{ 4 , 2 , 3 , 1 },
{ 4 , 3 , 1 , 2 },
{ 4 , 3 , 2 , 1 } };
const ordinal_type refEdgeOrts[24][4] = { { 0, 0, 0, 1 },
{ 1, 0, 0, 1 },
{ 0, 1, 0, 1 },
{ 0, 1, 0, 1 },
{ 1, 0, 0, 1 },
{ 1, 1, 0, 1 },
{ 0, 0, 1, 1 },
{ 1, 0, 1, 1 },
{ 0, 0, 1, 1 },
{ 0, 0, 1, 0 },
{ 1, 0, 1, 0 },
{ 1, 0, 1, 0 },
{ 0, 1, 0, 1 },
{ 0, 1, 0, 1 },
{ 0, 1, 1, 1 },
{ 0, 1, 1, 0 },
{ 0, 1, 0, 0 },
{ 0, 1, 1, 0 },
{ 1, 0, 0, 0 },
{ 1, 1, 0, 0 },
{ 1, 0, 1, 0 },
{ 1, 0, 1, 0 },
{ 1, 1, 0, 0 },
{ 1, 1, 1, 0 } };
for (auto i=0;i<24;++i) {
// find orientation
const auto nodes = Kokkos::View<const ordinal_type[4],HostSpaceType>(elemNodes[i]);
const auto ort = Orientation::getOrientation(cellTopo, nodes);
// decode orientation
ordinal_type edgeOrt[4] = {};
for (auto edgeId=0;edgeId<4;++edgeId)
ort.getEdgeOrientation(edgeOrt, 4);
*outStream << " elemNodes = "
<< elemNodes[i][0] << " "
<< elemNodes[i][1] << " "
<< elemNodes[i][2] << " "
<< elemNodes[i][3] << " :: "
<< " computed edgeOrts = "
<< edgeOrt[0] << " "
<< edgeOrt[1] << " "
<< edgeOrt[2] << " "
<< edgeOrt[3] << " :: "
<< " reference edgeOrts = "
<< refEdgeOrts[i][0] << " "
<< refEdgeOrts[i][1] << " "
<< refEdgeOrts[i][2] << " "
<< refEdgeOrts[i][3] << " ::\n";
if (edgeOrt[0] != refEdgeOrts[i][0] ||
edgeOrt[1] != refEdgeOrts[i][1] ||
edgeOrt[2] != refEdgeOrts[i][2] ||
edgeOrt[3] != refEdgeOrts[i][3]) {
*outStream << " ^^^^^^^^^^^^^^^^ FAILURE\n";
++errorFlag;
}
}
}
} catch (std::exception err) {
int main(int argc, char **argv)
{
ros::init(argc, argv, "state_publisher");
Orientation orientation;
orientation.loop();
}
void
BndryRegister::defineDoit (Orientation _face,
IndexType _typ,
int _in_rad,
int _out_rad,
int _extent_rad,
BoxArray& fsBA)
{
BL_PROFILE("BndryRegister::defineDoit()");
BL_ASSERT(grids.size() > 0);
const int coord_dir = _face.coordDir();
const int lo_side = _face.isLow();
//
// Build the BoxArray on which to define the FabSet on this face.
//
const int N = grids.size();
fsBA.resize(N);
#ifdef _OPENMP
#pragma omp parallel for
#endif
for (int idx = 0; idx < N; ++idx)
{
Box b;
//
// First construct proper box for direction normal to face.
//
if (_out_rad > 0)
{
if (_typ.ixType(coord_dir) == IndexType::CELL)
b = BoxLib::adjCell(grids[idx], _face, _out_rad);
else
b = BoxLib::bdryNode(grids[idx], _face, _out_rad);
if (_in_rad > 0)
b.grow(_face.flip(), _in_rad);
}
else
{
if (_in_rad > 0)
{
if (_typ.ixType(coord_dir) == IndexType::CELL)
b = BoxLib::adjCell(grids[idx], _face, _in_rad);
else
b = BoxLib::bdryNode(grids[idx], _face, _in_rad);
b.shift(coord_dir, lo_side ? _in_rad : -_in_rad);
}
else
BoxLib::Error("BndryRegister::define(): strange values for in_rad, out_rad");
}
//
// Now alter box in all other index directions.
//
for (int dir = 0; dir < BL_SPACEDIM; dir++)
{
if (dir == coord_dir)
continue;
if (_typ.ixType(dir) == IndexType::NODE)
b.surroundingNodes(dir);
if (_extent_rad > 0)
b.grow(dir,_extent_rad);
}
BL_ASSERT(b.ok());
fsBA.set(idx,b);
}
BL_ASSERT(fsBA.ok());
}
void
MCLinOp::applyBC (MultiFab& inout,
int level,
MCBC_Mode bc_mode)
{
//
// The inout MultiFab must have at least MCLinOp_grow ghost cells
// for applyBC()
//
BL_ASSERT(inout.nGrow() >= MCLinOp_grow);
//
// The inout MultiFab must have at least Periodic_BC_grow cells for the
// algorithms taking care of periodic boundary conditions.
//
BL_ASSERT(inout.nGrow() >= MCLinOp_grow);
//
// No coarsened boundary values, cannot apply inhomog at lev>0.
//
BL_ASSERT(!(level>0 && bc_mode == MCInhomogeneous_BC));
int flagden = 1; // fill in the bndry data and undrrelxr
int flagbc = 1; // with values
if (bc_mode == MCHomogeneous_BC)
flagbc = 0; // nodata if homog
int nc = inout.nComp();
BL_ASSERT(nc == numcomp );
inout.setBndry(-1.e30);
inout.FillBoundary();
prepareForLevel(level);
geomarray[level].FillPeriodicBoundary(inout,0,nc);
//
// Fill boundary cells.
//
#ifdef _OPENMP
#pragma omp parallel
#endif
for (MFIter mfi(inout); mfi.isValid(); ++mfi)
{
const int gn = mfi.index();
BL_ASSERT(gbox[level][gn] == inout.box(gn));
const BndryData::RealTuple& bdl = bgb.bndryLocs(gn);
const Array< Array<BoundCond> >& bdc = bgb.bndryConds(gn);
const MaskTuple& msk = maskvals[level][gn];
for (OrientationIter oitr; oitr; ++oitr)
{
const Orientation face = oitr();
FabSet& f = (*undrrelxr[level])[face];
FabSet& td = (*tangderiv[level])[face];
int cdr(face);
const FabSet& fs = bgb.bndryValues(face);
Real bcl = bdl[face];
const Array<BoundCond>& bc = bdc[face];
const int *bct = (const int*) bc.dataPtr();
const FArrayBox& fsfab = fs[gn];
const Real* bcvalptr = fsfab.dataPtr();
//
// Way external derivs stored.
//
const Real* exttdptr = fsfab.dataPtr(numcomp);
const int* fslo = fsfab.loVect();
const int* fshi = fsfab.hiVect();
FArrayBox& inoutfab = inout[gn];
FArrayBox& denfab = f[gn];
FArrayBox& tdfab = td[gn];
#if BL_SPACEDIM==2
int cdir = face.coordDir(), perpdir = -1;
if (cdir == 0)
perpdir = 1;
else if (cdir == 1)
perpdir = 0;
else
BoxLib::Abort("MCLinOp::applyBC(): bad logic");
const Mask& m = *msk[face];
const Mask& mphi = *msk[Orientation(perpdir,Orientation::high)];
const Mask& mplo = *msk[Orientation(perpdir,Orientation::low)];
FORT_APPLYBC(
&flagden, &flagbc, &maxorder,
inoutfab.dataPtr(),
ARLIM(inoutfab.loVect()), ARLIM(inoutfab.hiVect()),
&cdr, bct, &bcl,
bcvalptr, ARLIM(fslo), ARLIM(fshi),
m.dataPtr(), ARLIM(m.loVect()), ARLIM(m.hiVect()),
mphi.dataPtr(), ARLIM(mphi.loVect()), ARLIM(mphi.hiVect()),
mplo.dataPtr(), ARLIM(mplo.loVect()), ARLIM(mplo.hiVect()),
denfab.dataPtr(),
ARLIM(denfab.loVect()), ARLIM(denfab.hiVect()),
exttdptr, ARLIM(fslo), ARLIM(fshi),
tdfab.dataPtr(),ARLIM(tdfab.loVect()),ARLIM(tdfab.hiVect()),
inout.box(gn).loVect(), inout.box(gn).hiVect(),
&nc, h[level]);
#elif BL_SPACEDIM==3
const Mask& mn = *msk[Orientation(1,Orientation::high)];
const Mask& me = *msk[Orientation(0,Orientation::high)];
const Mask& mw = *msk[Orientation(0,Orientation::low)];
const Mask& ms = *msk[Orientation(1,Orientation::low)];
const Mask& mt = *msk[Orientation(2,Orientation::high)];
const Mask& mb = *msk[Orientation(2,Orientation::low)];
FORT_APPLYBC(
&flagden, &flagbc, &maxorder,
inoutfab.dataPtr(),
ARLIM(inoutfab.loVect()), ARLIM(inoutfab.hiVect()),
&cdr, bct, &bcl,
bcvalptr, ARLIM(fslo), ARLIM(fshi),
mn.dataPtr(),ARLIM(mn.loVect()),ARLIM(mn.hiVect()),
me.dataPtr(),ARLIM(me.loVect()),ARLIM(me.hiVect()),
mw.dataPtr(),ARLIM(mw.loVect()),ARLIM(mw.hiVect()),
ms.dataPtr(),ARLIM(ms.loVect()),ARLIM(ms.hiVect()),
mt.dataPtr(),ARLIM(mt.loVect()),ARLIM(mt.hiVect()),
mb.dataPtr(),ARLIM(mb.loVect()),ARLIM(mb.hiVect()),
denfab.dataPtr(),
ARLIM(denfab.loVect()), ARLIM(denfab.hiVect()),
exttdptr, ARLIM(fslo), ARLIM(fshi),
tdfab.dataPtr(),ARLIM(tdfab.loVect()),ARLIM(tdfab.hiVect()),
inout.box(gn).loVect(), inout.box(gn).hiVect(),
&nc, h[level]);
#endif
}
}
#if 0
// This "probably" works, but is not strictly needed just because of the way Bill
// coded up the tangential derivative stuff. It's handy code though, so I want to
// keep it around/
// Clean up corners:
// The problem here is that APPLYBC fills only grow cells normal to the boundary.
// As a result, any corner cell on the boundary (either coarse-fine or fine-fine)
// is not filled. For coarse-fine, the operator adjusts itself, sliding away from
// the box edge to avoid referencing that corner point. On the physical boundary
// though, the corner point is needed. Particularly if a fine-fine boundary intersects
// the physical boundary, since we want the stencil to be independent of the box
// blocking. FillBoundary operations wont fix the problem because the "good"
// data we need is living in the grow region of adjacent fabs. So, here we play
// the usual games to treat the newly filled grow cells as "valid" data.
// Note that we only need to do something where the grids touch the physical boundary.
const Geometry& geomlev = geomarray[level];
const BoxArray& grids = inout.boxArray();
const Box& domain = geomlev.Domain();
int nGrow = 1;
int src_comp = 0;
int num_comp = BL_SPACEDIM;
// Lets do a quick check to see if we need to do anything at all here
BoxArray BIGba = BoxArray(grids).grow(nGrow);
if (! (domain.contains(BIGba.minimalBox())) ) {
BoxArray boundary_pieces;
Array<int> proc_idxs;
Array<Array<int> > old_to_new(grids.size());
const DistributionMapping& dmap=inout.DistributionMap();
for (int d=0; d<BL_SPACEDIM; ++d) {
if (! (geomlev.isPeriodic(d)) ) {
BoxArray gba = BoxArray(grids).grow(d,nGrow);
for (int i=0; i<gba.size(); ++i) {
BoxArray new_pieces = BoxLib::boxComplement(gba[i],domain);
int size_new = new_pieces.size();
if (size_new>0) {
int size_old = boundary_pieces.size();
boundary_pieces.resize(size_old+size_new);
proc_idxs.resize(boundary_pieces.size());
for (int j=0; j<size_new; ++j) {
boundary_pieces.set(size_old+j,new_pieces[j]);
proc_idxs[size_old+j] = dmap[i];
old_to_new[i].push_back(size_old+j);
}
}
}
}
}
proc_idxs.push_back(ParallelDescriptor::MyProc());
MultiFab boundary_data(boundary_pieces,num_comp,nGrow,
DistributionMapping(proc_idxs));
for (MFIter mfi(inout); mfi.isValid(); ++mfi) {
const FArrayBox& src_fab = inout[mfi];
for (int j=0; j<old_to_new[mfi.index()].size(); ++j) {
int new_box_idx = old_to_new[mfi.index()][j];
boundary_data[new_box_idx].copy(src_fab,src_comp,0,num_comp);
}
}
boundary_data.FillBoundary();
// Use a hacked Geometry object to handle the periodic intersections for us.
// Here, the "domain" is the plane of cells on non-periodic boundary faces.
// and there may be cells over the periodic boundary in the remaining directions.
// We do a Geometry::PFB on each non-periodic face to sync these up.
if (geomlev.isAnyPeriodic()) {
Array<int> is_per(BL_SPACEDIM,0);
for (int d=0; d<BL_SPACEDIM; ++d) {
is_per[d] = geomlev.isPeriodic(d);
}
for (int d=0; d<BL_SPACEDIM; ++d) {
if (! is_per[d]) {
Box tmpLo = BoxLib::adjCellLo(geomlev.Domain(),d,1);
Geometry tmpGeomLo(tmpLo,&(geomlev.ProbDomain()),(int)geomlev.Coord(),is_per.dataPtr());
tmpGeomLo.FillPeriodicBoundary(boundary_data);
Box tmpHi = BoxLib::adjCellHi(geomlev.Domain(),d,1);
Geometry tmpGeomHi(tmpHi,&(geomlev.ProbDomain()),(int)geomlev.Coord(),is_per.dataPtr());
tmpGeomHi.FillPeriodicBoundary(boundary_data);
}
}
}
for (MFIter mfi(inout); mfi.isValid(); ++mfi) {
int idx = mfi.index();
FArrayBox& dst_fab = inout[mfi];
for (int j=0; j<old_to_new[idx].size(); ++j) {
int new_box_idx = old_to_new[mfi.index()][j];
const FArrayBox& src_fab = boundary_data[new_box_idx];
const Box& src_box = src_fab.box();
BoxArray pieces_outside_domain = BoxLib::boxComplement(src_box,domain);
for (int k=0; k<pieces_outside_domain.size(); ++k) {
const Box& outside = pieces_outside_domain[k] & dst_fab.box();
if (outside.ok()) {
dst_fab.copy(src_fab,outside,0,outside,src_comp,num_comp);
}
}
}
}
}
#endif
}
