Assembly *GraspGLObjects::CreateRollPrompt( double radius ) {
Assembly *prompt = new Assembly();
// Angular extent of the circular arrow, where 1.0 = 360°.
double guage = radius / 10.0;
double arc = 0.85;
Annulus *donut = new Annulus( radius, guage, arc, curve_facets, curve_facets );
donut->SetAttitude( 0.0, 90.0, 0.0 );
prompt->AddComponent( donut );
TaperedAnnulus *tip = new TaperedAnnulus( radius, radius / 3.0, 1.0, 0.05, curve_facets );
tip->SetAttitude( 0.0, 90.0, 0.0 );
tip->SetOrientation( - arc * 360.0, 0.0, 0.0 );
prompt->AddComponent( tip );
Ellipsoid *base = new Ellipsoid ( guage, guage / 2.0, guage );
base->SetPosition( radius, 0.0, 0.0 );
prompt->AddComponent( base );
prompt->SetColor( 0.5, 0.5, 0.5, 0.5 );
return prompt;
}
void EllipsoidLibraryImplementation::ellipsoidParameters( const long index, double *a, double *f )
{
/*
* The function ellipsoidParameters returns the semi-major axis and flattening
* for the ellipsoid with the specified index. If index is invalid,
* exception is thrown.
*
* index : Index of a given ellipsoid in the ellipsoid table (input)
* a : Semi-major axis, in meters, of ellipsoid (output)
* f : Flattening of ellipsoid. (output)
*
*/
*a = 0;
*f = 0;
if( ( index < 0 ) || ( index >= ellipsoidList.size() ) )
throw CoordinateConversionException( ErrorMessages::invalidIndex );
else
{
Ellipsoid* ellipsoid = ellipsoidList[index];
*a = ellipsoid->semiMajorAxis();
*f = ellipsoid->flattening();
}
}
IlwisObject *InternalIlwisObjectFactory::createEllipsoidFromQuery(const QString &query, const Resource& resource) const {
if ( query == "") {
return createFromResource<Ellipsoid>(resource, IOOptions());
}
InternalDatabaseConnection db(24);
if (db.exec(query) && db.next()) {
Ellipsoid *ellipsoid = createFromResource<Ellipsoid>(resource, IOOptions());
ellipsoid->fromInternal(db.record());
return ellipsoid;
}
return 0;
}
void projectEllipsoid(Ellipsoid& ell, double ell_height, const PCRGB& pc, PCRGB& pc_ell)
{
double lat, lon, height, x, y, z, h, s, l;
BOOST_FOREACH(PRGB const pt, pc.points) {
PRGB npt;
extractHSL(pt.rgb, h, s, l);
ell.cartToEllipsoidal(pt.x, pt.y, pt.z, lat, lon, height);
ell.ellipsoidalToCart(lat, lon, ell_height, x, y, z);
npt.x = x; npt.y = y; npt.z = z;
writeHSL(0, 0, l, npt.rgb);
pc_ell.points.push_back(npt);
}
pc_ell.header.frame_id = "base_link";
//pubLoop(pc_ell, "/proj_head");
}
//----------------------------------------------------------------------------
bool Mgc::Culled (const Plane& rkPlane, const Ellipsoid& rkEllipsoid,
bool bUnitNormal)
{
Vector3 kNormal = rkPlane.Normal();
Real fConstant = rkPlane.Constant();
if ( !bUnitNormal )
{
Real fLength = kNormal.Unitize();
fConstant /= fLength;
}
Real fDiscr = kNormal.Dot(rkEllipsoid.InverseA()*kNormal);
Real fRoot = Math::Sqrt(Math::FAbs(fDiscr));
Real fSDist = kNormal.Dot(rkEllipsoid.Center()) - fConstant;
return fSDist <= -fRoot;
}
void wrenchCallback(geometry_msgs::WrenchStamped::ConstPtr wrench_stamped)
{
double cur_time = wrench_stamped->header.stamp.toSec();
if(start_time == -1)
start_time = cur_time;
CartVec w;
wrenchMsgToEigen(wrench_stamped->wrench, w);
double force_mag = NORM(w(0, 0), w(1, 0), w(2, 0));
tf::StampedTransform tool_loc_tf;
try {
tf_list.waitForTransform("/head_center", "/wipe_finger", wrench_stamped->header.stamp, ros::Duration(0.1));
tf_list.lookupTransform("/head_center", "/wipe_finger", wrench_stamped->header.stamp, tool_loc_tf);
}
catch (tf::TransformException ex) {
ROS_ERROR("%s", ex.what());
return;
}
tool_loc_tf.mult(registration_tf, tool_loc_tf);
btVector3 tool_loc = tool_loc_tf.getOrigin();
PRGB query_pt;
query_pt.x = tool_loc.x(); query_pt.y = tool_loc.y(); query_pt.z = tool_loc.z();
vector<int> nk_inds(1);
vector<float> nk_dists(1);
kd_tree->nearestKSearch(query_pt, 1, nk_inds, nk_dists);
int closest_ind = nk_inds[0];
float closest_dist = nk_dists[0];
hrl_phri_2011::ForceProcessed fp;
fp.time_offset = cur_time - start_time;
tf::transformStampedTFToMsg(tool_loc_tf, fp.tool_frame);
fp.header = wrench_stamped->header;
fp.header.frame_id = "/head_center";
fp.wrench = wrench_stamped->wrench;
fp.pc_ind = closest_ind;
fp.pc_dist = closest_dist;
fp.pc_pt.x = pc_head->points[closest_ind].x;
fp.pc_pt.y = pc_head->points[closest_ind].y;
fp.pc_pt.z = pc_head->points[closest_ind].z;
fp.force_magnitude = force_mag;
if(compute_norms) {
fp.pc_normal.x = normals->points[closest_ind].normal[0];
fp.pc_normal.y = normals->points[closest_ind].normal[1];
fp.pc_normal.z = normals->points[closest_ind].normal[2];
}
// do ellipsoidal processing
tf::Transform tool_loc_ell = ell_reg_tf * tool_loc_tf;
tf::transformTFToMsg(tool_loc_ell, fp.tool_ell_frame);
btVector3 tloce_pos = tool_loc_ell.getOrigin();
ell.cartToEllipsoidal(tloce_pos.x(), tloce_pos.y(), tloce_pos.z(),
fp.ell_coords.x, fp.ell_coords.y, fp.ell_coords.z);
fp_list.push_back(fp);
pub_ind++;
if(pub_ind % 100 == 0)
ROS_INFO("Recorded %d samples", pub_ind);
}
IEllipsoid CoordinateSystemConnector::getEllipsoid() {
QString ell = _odf->value("CoordSystem","Ellipsoid");
if ( ell == "?")
return IEllipsoid();
if ( ell == "User Defined") {
double invf = _odf->value("Ellipsoid","1/f").toDouble();
double majoraxis = _odf->value("Ellipsoid","a").toDouble();
Ellipsoid *ellips = new Ellipsoid();
QString newName = ellips->setEllipsoid(majoraxis,invf);;
ellips->name(newName);
IEllipsoid ellipsoid(ellips);
return ellipsoid;
}
IEllipsoid ellipsoid;
QString code = name2Code(ell, "ellipsoid");
if ( code == sUNDEF)
return IEllipsoid();
QString resource = QString("ilwis://tables/ellipsoid?code=%1").arg(code);
ellipsoid.prepare(resource);
return ellipsoid;
}
// The following objects are not used during the Grasp protocol and are not seen by the subject.
// Rather, these objects are used in the GraspGUI and elswhere to visualize the subject's pose.
// Perhaps they would be better placed in another class so that they are not included when not needed.
Assembly *GraspGLObjects::CreateHead( void ) {
Assembly *head = new Assembly();
// Skull
Ellipsoid *skull = new Ellipsoid( head_shape );
skull->SetColor( .4f, 0.0f, .4f );
// Eyes
head->AddComponent( skull );
Sphere *sphere = new Sphere( 20.0 );
sphere->SetColor( 0.0f, 0.0f, 1.0f );
sphere->SetPosition( -50.0, 20.0, -100.0 );
head->AddComponent( sphere );
sphere = new Sphere( 20.0 );
sphere->SetColor( 0.0f, 0.0f, 1.0f );
sphere->SetPosition( 50.0, 20.0, -100.0 );
head->AddComponent( sphere );
// Nose
Cylinder *cylinder = new Cylinder( 20.0, 5.0, 30.0 );
cylinder->SetPosition( 0.0, -20.0, - head_shape[Z] );
cylinder->SetOrientation( 0.0, 90.0, 0.0 );
cylinder->SetColor( YELLOW );
head->AddComponent( cylinder );
return head;
}
bool EllipsoidConnector::createEllipsoidFromCode(const QString& code, IlwisObject *data) {
QString query = QString("Select * from ellipsoid where code = '%1'").arg(code);
QSqlQuery db(kernel()->database());
if (db.exec(query)) {
if ( db.next()) {
Ellipsoid *ellipsoid = static_cast<Ellipsoid *>(data);
ellipsoid->setName(db.record().field("name").value().toString());
ellipsoid->setDescription(db.record().field("description").value().toString());
double ma = db.record().field("majoraxis").value().toDouble();
double rf = db.record().field("invflattening").value().toDouble();
ellipsoid->setEllipsoid(ma,rf);
ellipsoid->setAuthority(db.record().field("authority").value().toString());
ellipsoid->setConnector(this); // we are internal, no connector needed
ellipsoid->setCode(code);
return true;
}
}else
kernel()->issues()->logSql(db.lastError());
return false;
}
void projectEllipsoidDense(Ellipsoid& ell, double ell_height, const PCRGB& pc,
int num_lat, int num_lon, double sigma, int k)
{
pcl::KdTreeFLANN<PRGB> kd_tree(new pcl::KdTreeFLANN<PRGB> ());
kd_tree.setInputCloud(pc.makeShared());
PCRGB pc_dense;
vector<int> inds;
vector<float> dists;
double h, s, l, h_sum, s_sum, l_sum, normal, normal_sum;
double x, y, z;
double lat = 0, lon = 0;
for(int i=0;i<num_lat;i++) {
lat += PI / num_lat;
lon = 0;
for(int j=0;j<num_lon;j++) {
lon += 2 * PI / num_lon;
PRGB pt;
ell.ellipsoidalToCart(lat, lon, ell_height, x, y, z);
pt.x = x; pt.y = y; pt.z = z;
inds.clear();
dists.clear();
kd_tree.nearestKSearch(pt, k, inds, dists);
normal_sum = 0; h_sum = 0; s_sum = 0; l_sum = 0;
for(uint32_t inds_i=0;inds_i<inds.size();inds_i++) {
extractHSL(pc.points[inds[inds_i]].rgb, h, s, l);
normal = NORMAL(dists[inds_i], sigma);
normal_sum += normal;
h_sum += normal * h; s_sum += normal * s; l_sum += normal * l;
}
writeHSL(h_sum / normal_sum, s_sum / normal_sum, l_sum / normal_sum, pt.rgb);
pc_dense.points.push_back(pt);
}
}
pc_dense.header.frame_id = "/base_link";
pubLoop(pc_dense, "/proj_head");
}
bool Ellipsoid::overlapsWith(Ellipsoid ellipsoid, bool &ellipsoidMatrixDecompositionIsSuccessful)
{
// Construct translation matrix
MatrixXd T1 = MatrixXd::Identity(Ndimensions+1,Ndimensions+1);
MatrixXd T2 = MatrixXd::Identity(Ndimensions+1,Ndimensions+1);
T1.bottomLeftCorner(1,Ndimensions) = (-1.0) * centerCoordinates.transpose();
T2.bottomLeftCorner(1,Ndimensions) = (-1.0) * ellipsoid.getCenterCoordinates().transpose();
// Construct ellipsoid matrix in homogeneous coordinates
MatrixXd A = MatrixXd::Zero(Ndimensions+1,Ndimensions+1);
MatrixXd B = A;
A(Ndimensions,Ndimensions) = -1;
B(Ndimensions,Ndimensions) = -1;
A.topLeftCorner(Ndimensions,Ndimensions) = covarianceMatrix.matrix().inverse();
B.topLeftCorner(Ndimensions,Ndimensions) = ellipsoid.getCovarianceMatrix().matrix().inverse();
MatrixXd AT = T1*A*T1.transpose(); // Translating to ellipsoid center
MatrixXd BT = T2*B*T2.transpose(); // Translating to ellipsoid center
// Compute Hyper Quadric Matrix generating from the two ellipsoids
// and derive its eigenvalues decomposition
MatrixXd C = AT.inverse() * BT;
MatrixXcd CC(Ndimensions+1,Ndimensions+1);
CC.imag() = MatrixXd::Zero(Ndimensions+1,Ndimensions+1);
CC.real() = C;
ComplexEigenSolver<MatrixXcd> eigenSolver(CC);
// If eigenvalue decomposition fails, set control flag to false
// to stop the nested sampling and print the results
if (eigenSolver.info() != Success)
{
ellipsoidMatrixDecompositionIsSuccessful = false;
}
MatrixXcd E = eigenSolver.eigenvalues();
MatrixXcd V = eigenSolver.eigenvectors();
bool ellipsoidsDoOverlap = false; // Assume no overlap in the beginning
double pointA; // Point laying in this ellipsoid
double pointB; // Point laying in the other ellipsoid
// Loop over all eigenvectors
for (int i = 0; i < Ndimensions+1; i++)
{
// Skip inadmissible eigenvectors
if (V(Ndimensions,i).real() == 0)
{
continue;
}
else if (E(i).imag() != 0)
{
V.col(i) = V.col(i).array() * (V.conjugate())(Ndimensions,i); // Multiply eigenvector by complex conjugate of last element
V.col(i) = V.col(i).array() / V(Ndimensions,i).real(); // Normalize eigenvector to last component value
pointA = V.col(i).transpose().real() * AT * V.col(i).real(); // Evaluate point from this ellipsoid
pointB = V.col(i).transpose().real() * BT * V.col(i).real(); // Evaluate point from the other ellipsoid
// Accept only if point belongs to both ellipsoids
if ((pointA <= 0) && (pointB <= 0))
{
ellipsoidsDoOverlap = true; // Exit if ellipsoidsDoOverlap is found
break;
}
}
}
return ellipsoidsDoOverlap;
}
// Inicializacio, a program futasanak kezdeten, az OpenGL kontextus letrehozasa utan hivodik meg (ld. main() fv.)
void onInitialization() {
glViewport(0, 0, screenWidth, screenHeight);
Color ks = Color(0.4, 0.4, 0.4);
Color kd = Color(255, 215, 0) / 255;
Color k = Color(3.1, 2.7, 1.9);
Color k_1 = Color(1.1, 1.7, 0.9);
Color n = Color(0.17, 0.35, 1.5);
Material *material = new Material(ks, Color(255, 200, 50) / 255, n, k,
Color(), 50, false, false);
Material *material_2 = new Material(ks, Color(205, 127, 50) / 255, n, k,
Color(), 50, false, false);
Material *material_3 = new Material(ks, Color(0, 178, 89) / 255, n, k,
Color(), 50, true, false);
Material *material_4 = new Material(ks, Color(0, 144, 244) / 255,
Color(1.5, 1.5, 1.5), Color(), Color(), 50, false, true);
// Sphere implementation----------------------------------------------
Sphere *firstSphere = new Sphere(material_4);
// Ellipsoid implementation-------------------------------------------
Ellipsoid *firstEllipsoid = new Ellipsoid(material);
myMatrix transfom1 = myMatrix(0.3, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0.3, 0, 0,
0, 0, 1);
myMatrix transfom3 = myMatrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, -0.2, 0,
0.5, 1);
myMatrix transfom2 = myMatrix(cos(M_PI / 6), sin(M_PI / 6), 0, 0,
-sin(M_PI / 6), cos(M_PI / 6), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
myMatrix transform4 = myMatrix(0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0.3, 0.2,
1, 1);
Vector normal = Vector(-0.529863, 0.253724, 0.80924, 1);
Vector origo = Vector(-0.150579, 0.20029, 0.229974, 0);
Hit hit = Hit();
hit.normalVector = normal;
hit.intersectPoint = origo;
myMatrix tr1 = myMatrix(0.15, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0.15, 0, 0, 0,
0, 1);
myMatrix tr3 = myMatrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0.25, 0, 1);
myMatrix tr = getTheNewCoordSys(hit);
Ellipsoid *secondEllipsoid = new Ellipsoid(material);
myMatrix first_connection_matrix = tr1 * tr3 * tr;
firstEllipsoid->setTrasformationMatrix(transfom1);
secondEllipsoid->setTrasformationMatrix(first_connection_matrix);
Vector normal_2 = normal * first_connection_matrix;
Vector origo_2 = origo * first_connection_matrix;
hit.normalVector = normal_2;
hit.intersectPoint = origo_2;
tr1 = myMatrix(0.15 , 0, 0, 0, 0, 0.25 , 0, 0, 0, 0,
0.15 , 0, 0, 0, 0, 1);
tr3 = myMatrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0.25 , 0,
1);
tr = getTheNewCoordSys(hit);
Ellipsoid *thirdEllipsoid = new Ellipsoid(material);
myMatrix second_connection_matrix = first_connection_matrix*tr;
thirdEllipsoid->setTrasformationMatrix(second_connection_matrix);
// Cylinder implementation---------------------------------------------
Cylinder *firstCylinder = new Cylinder(material);
transfom1 = myMatrix(0.2, 0, 0, 0, 0, 0.2, 0, 0, 0, 0, 0.2, 0, 0, 0, 0, 1);
transfom3 = myMatrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0.4, 0, -0.6, 1);
transfom2 = myMatrix(cos(M_PI / 2), sin(M_PI / 2), 0, 0, -sin(M_PI / 2),
cos(M_PI / 2), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
firstCylinder->setTrasformationMatrix((transfom1 * transfom2) * transfom3);
// Paraboloid implemenation--------------------------------------------
Paraboloid *firstParaboloid = new Paraboloid(material_4);
transfom1 = myMatrix(0.2, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0.2, 0, 0, 0, 0, 1);
transfom3 = myMatrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0.4, 0, -0.6, 1);
transfom2 = myMatrix(cos(M_PI / 2), sin(M_PI / 2), 0, 0, -sin(M_PI / 2),
cos(M_PI / 2), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
firstParaboloid->setTrasformationMatrix((transfom1) * transfom3);
// Plane implementation------------------------------------------------
Plane *firstPlane = new Plane(material_2);
Plane2 *secondPlane = new Plane2(material_2);
Scene scene = Scene();
scene.AddObject((Intersectable*) firstPlane);
// scene.AddObject((Intersectable*) firstCylinder);
// scene.AddObject((Intersectable*) firstSphere);
scene.AddObject((Intersectable*) firstEllipsoid);
scene.AddObject((Intersectable*) secondEllipsoid);
// scene.AddObject((Intersectable*) thirdEllipsoid);
// scene.AddObject((Intersectable*) secondPlane);
// scene.AddObject((Intersectable*) firstParaboloid);
scene.SetAmbientColor(Color(77, 199, 253) / 255);
PositionLightSource *light = new PositionLightSource(Vector(-1, 6, 7),
Color(1, 1, 1));
scene.AddLight((LightSource*) light);
MyCamera camera = MyCamera(Vector(1, 2, 3), Vector(0, 0, 0),
Vector(0, 1, 0));
scene.SetCamera(camera);
scene.render();
}
C3DModel CTriangulator<Real, Ellipsoid<Real> >::Triangulate(const Ellipsoid<Real> &pShape)
{
//-Pi/2 to Pi/2
Real phi;
//0 to 2Pi
Real theta;
//points on the sphere
//x=x0+r*cos(theta)*cos(phi)
//y=y0+r*cos(theta)*sin(phi)
//z=z0+r*sin(theta)
C3DModel model;
std::vector<Vector3<Real> > vVertices;
std::vector<TriFace> vFaces;
int lat =8;
int longi=8;
Real dphi = CMath<Real>::SYS_PI/(Real)longi;
Real dtheta = CMath<Real>::SYS_PI/(Real)lat;
Real halfpi = CMath<Real>::SYS_PI/2.0;
Vector3<Real> vTop=pShape.eval(halfpi,0);
Vector3<Real> vBottom=pShape.eval(-halfpi,0);
vVertices.push_back(vTop);
phi = halfpi-dphi;
for(int j=1;j<longi;j++)
{
theta=0.0f;
for(int i=0;i<2*lat;i++)
{
Vector3<Real> vNext=pShape.eval(phi,theta);
vVertices.push_back(vNext);
theta+=dtheta;
}//end for i
phi-=dphi;
}//end for j
vVertices.push_back(vBottom);
// for(int i=0;i<vVertices.size();i++)
//cout<<vVertices[i]<<endl;
int lat2=2*lat;
//add upper triangle fan
for(int i=0;i<lat2;i++)
{
int verts[3];
verts[0]=0;
verts[1]=1+i;
verts[2]=1+(i+1)%lat2;
TriFace face(verts);
vFaces.push_back(face);
}
//add body
for(int i=0;i<longi-2;i++)
{
int index=1+i*lat2;
for(int j=0;j<lat2;j++)
{
int verts[3];
verts[0]=index+j;
verts[1]=index+lat2+j;
verts[2]=index+(j+1)%lat2;
TriFace face1(verts);
vFaces.push_back(face1);
verts[0]=index+(j+1)%lat2;
verts[1]=index+lat2+j;
verts[2]=index+lat2+(j+1)%lat2;
TriFace face2(verts);
vFaces.push_back(face2);
}
}
int ilast=vVertices.size()-1;
int ilastrow=ilast-lat2;
//add lower triangle fan
for(int i=0;i<lat2;i++)
{
int verts[3];
verts[0]=ilast;
verts[1]=ilastrow+(i+1)%lat2;
verts[2]=ilastrow+i;
TriFace face(verts);
vFaces.push_back(face);
}
model.CreateFrom(vVertices,vFaces);
return model;
}
bool Segment::Intersects( const Ellipsoid& e, CollisionInfo* const pInfo /*= NULL*/ ) const
{
return e.Intersects( *this, pInfo );
}
void VoxelGrid::
paintEllipsoid(const GridDescription & gridDesc,
const Ellipsoid & instruction)
{
int ii, jj, kk, iYee, jYee, kYee;
// LOG << "Warning: this probably has bugs in it.\n";
Paint* paint = Paint::paint(instruction.material());
if (instruction.fillStyle() == kPECStyle ||
instruction.fillStyle() == kPMCStyle ||
instruction.fillStyle() == kYeeCellStyle)
{
//halfCells = instruction.yeeRect();
//halfCells.p2 += Vector3i(1,1,1);
Rect3i yeeRect = instruction.yeeRect();
Vector3i centerTimesTwo = yeeRect.p1 + yeeRect.p2;
Vector3i extent = yeeRect.p2 - yeeRect.p1 + Vector3i(1,1,1);
Vector3d radii = 0.5*Vector3d(extent[0], extent[1], extent[2]);
Vector3d center = 0.5*Vector3d(centerTimesTwo[0], centerTimesTwo[1],
centerTimesTwo[2]);
// Don't clip: we clip in the paint routines.
//Rect3i fillRect(clip(gridDesc.yeeBounds(), yeeRect.p1),
// clip(gridDesc.yeeBounds(), yeeRect.p2));
Rect3i fillRect(yeeRect);
for (kYee = fillRect.p1[2]; kYee <= fillRect.p2[2]; kYee++)
for (jYee = fillRect.p1[1]; jYee <= fillRect.p2[1]; jYee++)
for (iYee = fillRect.p1[0]; iYee <= fillRect.p2[0]; iYee++)
{
Vector3d v( Vector3d(iYee,jYee,kYee) - center );
v[0] /= radii[0];
v[1] /= radii[1];
v[2] /= radii[2];
if (norm2(v) <= 1.00001) // give it room for error
{
if (instruction.fillStyle() == kPECStyle)
paintPEC(paint, iYee, jYee, kYee);
else if (instruction.fillStyle() == kPMCStyle)
paintPMC(paint, iYee, jYee, kYee);
else
paintYeeCell(paint, iYee, jYee, kYee);
}
}
}
else if (instruction.fillStyle() == kHalfCellStyle)
{
Rect3i halfCells = instruction.yeeRect();
halfCells.p2 += Vector3i(1,1,1);
halfCells = halfCells;
Vector3i extent = halfCells.p2 - halfCells.p1 + Vector3i(1,1,1);
Vector3i center2 = halfCells.p1 + halfCells.p2;
Vector3d radii = 0.5*Vector3d(extent[0], extent[1], extent[2]);
Vector3d center = 0.5*Vector3d(center2[0], center2[1], center2[2]);
// Don't clip: we clip in the paint routines
//halfCells = clip(halfCells, mAllocRegion);
if (instruction.fillStyle() == kHalfCellStyle)
{
for (kk = halfCells.p1[2]; kk <= halfCells.p2[2]; kk++)
for (jj = halfCells.p1[1]; jj <= halfCells.p2[1]; jj++)
for (ii = halfCells.p1[0]; ii <= halfCells.p2[0]; ii++)
{
Vector3d v( Vector3d(ii,jj,kk) - center );
v[0] /= radii[0];
v[1] /= radii[1];
v[2] /= radii[2];
if (norm2(v) <= 1.00001) // give it room for error
(*this)(ii,jj,kk) = paint;
}
}
}
}