void LargeScaleOctree::insert(Vertexf& pos)
{
if(isLeaf())
{
m_data.addBuffered(pos);
if(m_data.size() == m_maxPoints)
{
m_data.writeBuffer();
m_leaf = false;
//cout << "NEW NODES" << endl;
for(int i = 0 ; i<8 ; i++)
{
Vertexf newCenter;
newCenter.x = m_center.x + m_size * 0.5 * (i&4 ? 0.5 : -0.5);
newCenter.y = m_center.y + m_size * 0.5 * (i&2 ? 0.5 : -0.5);
newCenter.z = m_center.z + m_size * 0.5 * (i&1 ? 0.5 : -0.5);
m_children.push_back( new LargeScaleOctree(newCenter, m_size * 0.5, m_maxPoints) );
}
for(Vertexf v : m_data)
{
m_children[getOctant(v)]->insert(v);
}
m_data.remove();
}
}
else
{
m_children[getOctant(pos)]->insert(pos);
}
}
void Octree::add(OcPoint p, void* data, int maxjump) {
bool rOk = checkRecursion(this, p);
if (!rOk) {
cout << "\n Center "; center.print();
cout << "\n Size " << size;
cout << "\n Data size " << this->data.size();
cout << "\n In Box: " << p.inBox(center, size) << endl;
cout << "\n P Center "; parent->center.print();
cout << "\n P Size " << parent->size;
cout << "\n In Box: " << p.inBox(parent->center, parent->size) << endl;
cout << "\n P OcPoint Octant " << parent->getOctant(p);
cout << "\n P child at OcPoint Octant " << parent->children[ parent->getOctant(p) ];
cout << "\n this " << this;
return;
}
//cout << "\nAdd "; p.print();
OcPoint rp = p.sub(center);
if ( ! p.inBox(center, size) ) {
if (parent == 0) {
float s2 = size*0.5;
parent = new Octree(resolution);
//OcPoint c = center.add( OcPoint(copysign(s2,rp.x), copysign(s2,rp.y), copysign(s2,rp.z)) );
OcPoint c = center.add( lvljumpCenter(s2, rp) );
parent->center = c;
float s = 2*size;
parent->size = s;
int o = parent->getOctant(center);
parent->children[o] = this;
}
parent->add(p, data);
return;
}
if (size > resolution && maxjump != 0) {
int o = getOctant(p);
if (children[o] == 0) {
float s2 = size*0.25;
children[o] = new Octree(resolution);
float s = size*0.5;
children[o]->size = s;
//OcPoint c = center.add( OcPoint(copysign(s2,rp.x), copysign(s2,rp.y), copysign(s2,rp.z)) );
OcPoint c = center.add( lvljumpCenter(s2, rp) );
children[o]->center = c;
children[o]->parent = this;
}
//OcPoint c = children[o]->center;
children[o]->add(p, data, maxjump-1);
return;
}
checkRecursion(0,p);
this->data.push_back(data);
}
Octree* Octree::get(float x, float y, float z) {
OcPoint p = OcPoint(x,y,z);
if ( ! p.inBox(center, size) ) {
return parent == 0 ? 0 : parent->get(x,y,z);
}
if (size > resolution) {
int o = getOctant(p);
if (children[o] == 0) return this;
return children[o]->get(x,y,z);
}
return this;
}
//=================================================================================
void computeCell(float *xcoord, float *ycoord, float *zcoord,
int *coordInBox, int numCoordInBox,
float bbx1, float bby1, float bbz1,
float bbx2, float bby2, float bbz2,
int maxCoord, int *replBy, int &numCoordToRemove)
{
int i, j;
int v, w;
float rx, ry, rz;
float obx1, oby1, obz1;
float obx2, oby2, obz2;
int numCoordInCell[8];
int *coordInCell[8];
// too many Coords in my box -> split octree box deeper
if (numCoordInBox > maxCoord)
{
// yes we have
rx = (bbx1 + bbx2) / 2.0f;
ry = (bby1 + bby2) / 2.0f;
rz = (bbz1 + bbz2) / 2.0f;
// go through the coordinates and sort them in the right cell
for (i = 0; i < 8; i++)
{
coordInCell[i] = new int[numCoordInBox];
numCoordInCell[i] = 0;
}
for (i = 0; i < numCoordInBox; i++)
{
v = coordInBox[i];
w = getOctant(xcoord[v], ycoord[v], zcoord[v], rx, ry, rz);
coordInCell[w][numCoordInCell[w]] = v;
numCoordInCell[w]++;
}
// we're recursive - hype
for (i = 0; i < 8; i++)
{
if (numCoordInCell[i])
{
if (numCoordInCell[i] > numCoordInBox / 4)
{
// we decide to compute the new BoundingBox instead of
// just splitting the parent-Box
boundingBox(&xcoord, &ycoord, &zcoord, coordInCell[i],
numCoordInCell[i], &obx1, &oby1, &obz1,
&obx2, &oby2, &obz2);
}
else
getOctantBounds(i, rx, ry, rz, bbx1, bby1, bbz1,
bbx2, bby2, bbz2,
&obx1, &oby1, &obz1, &obx2, &oby2, &obz2);
computeCell(xcoord, ycoord, zcoord, coordInCell[i],
numCoordInCell[i], obx1, oby1, obz1,
obx2, oby2, obz2, maxCoord, replBy, numCoordToRemove);
}
delete[] coordInCell[i];
}
}
//// directly compare in box
else if (numCoordInBox > 1)
{
// check these vertices
for (i = 0; i < numCoordInBox - 1; i++)
{
v = coordInBox[i];
rx = xcoord[v];
ry = ycoord[v];
rz = zcoord[v];
// see if this one is doubled
for (j = i + 1; j < numCoordInBox; j++)
{
w = coordInBox[j];
if (xcoord[w] == rx && // @@@@ add distance fkt here if necessary
ycoord[w] == ry && zcoord[w] == rz)
{
// this one is double
if (v < w)
replBy[w] = v;
else
replBy[v] = w;
numCoordToRemove++;
// break out
j = numCoordInBox;
}
}
}
}
// done
return;
}
//=================================================================================
void computeCell(float *xcoord, float *ycoord, float *zcoord,
int *coordInBox, int numCoordInBox,
float bbx1, float bby1, float bbz1,
float bbx2, float bby2, float bbz2,
int optimize, float maxDistanceSqr, int maxCoord)
{
int i, j;
int v, w;
float rx, ry, rz;
float obx1, oby1, obz1;
float obx2, oby2, obz2;
int numCoordInCell[8];
int *coordInCell[8];
// check if we have to go any further
if (numCoordInBox > maxCoord)
{
// yes we have
rx = (bbx1 + bbx2) / 2.0f;
ry = (bby1 + bby2) / 2.0f;
rz = (bbz1 + bbz2) / 2.0f;
if (optimize)
{
// we have to mess with memory
for (i = 0; i < 8; i++)
numCoordInCell[i] = 0;
for (i = 0; i < numCoordInBox; i++)
{
v = coordInBox[i];
numCoordInCell[getOctant(xcoord[v], ycoord[v], zcoord[v], rx, ry, rz)]++;
}
for (i = 0; i < 8; i++)
{
if (numCoordInCell[i])
coordInCell[i] = new int[numCoordInCell[i]];
else
coordInCell[i] = NULL;
}
}
else
for (i = 0; i < 8; i++)
coordInCell[i] = new int[numCoordInBox];
// go through the coordinates and sort them in the right cell
for (i = 0; i < 8; i++)
numCoordInCell[i] = 0;
for (i = 0; i < numCoordInBox; i++)
{
v = coordInBox[i];
w = getOctant(xcoord[v], ycoord[v], zcoord[v], rx, ry, rz);
if (coordInCell[w] != NULL)
{
coordInCell[w][numCoordInCell[w]] = v;
numCoordInCell[w]++;
}
}
// we're recursive - hype
for (i = 0; i < 8; i++)
{
if (numCoordInCell[i])
{
if (numCoordInCell[i] > numCoordInBox / 4)
{
// we decide to compute the new BoundingBox instead of
// just splitting the parent-Box
boundingBox(&xcoord, &ycoord, &zcoord, coordInCell[i],
numCoordInCell[i], &obx1, &oby1, &obz1,
&obx2, &oby2, &obz2);
}
else
getOctantBounds(i, rx, ry, rz, bbx1, bby1, bbz1,
bbx2, bby2, bbz2,
&obx1, &oby1, &obz1, &obx2, &oby2, &obz2);
computeCell(xcoord, ycoord, zcoord, coordInCell[i],
numCoordInCell[i], obx1, oby1, obz1,
obx2, oby2, obz2, optimize, maxDistanceSqr, maxCoord);
}
delete[] coordInCell[i];
}
}
else if (numCoordInBox > 1)
{
// check these vertices
for (i = 0; i < numCoordInBox - 1; i++)
{
v = coordInBox[i];
rx = xcoord[v];
ry = ycoord[v];
rz = zcoord[v];
// see if this one is doubled
for (j = i + 1; j < numCoordInBox; j++)
{
w = coordInBox[j];
if (isEqual(xcoord[w], ycoord[w], zcoord[w], rx, ry, rz, maxDistanceSqr))
{
// this one is double
if (v < w)
{
replBy[w] = v;
//coordToRemove[0][numCoordToRemove] = w; // was w
//coordToRemove[1][numCoordToRemove] = v; // was v
}
else
{
replBy[v] = w;
//coordToRemove[0][numCoordToRemove] = w; // was w
//coordToRemove[1][numCoordToRemove] = v; // was v
}
numCoordToRemove++;
// break out
j = numCoordInBox;
}
}
}
}
// done
return;
}
void BHTreeCZBuilder::refineCZcell(const czllPtrT _czllPtr)
{
///
/// transform all children to empty CZ cells. if a child
/// is a particle, first link a empty cell in between.
///
curPtr = _czllPtr;
for (size_t i = 0; i < 8; i++)
{
if (static_cast<gcllPtrT>(curPtr)->child[i] == NULL)
{
static_cast<gcllPtrT>(curPtr)->child[i] = new czllT;
goChild(i);
static_cast<czllPtrT>(curPtr)->clear();
curPtr->parent = _czllPtr;
static_cast<gcllPtrT>(curPtr)->inheritCellPos(i);
goUp();
}
else if (static_cast<gcllPtrT>(curPtr)->child[i]->isParticle)
{
const pnodPtrT resPartPtr =
static_cast<pnodPtrT>(static_cast<gcllPtrT>(curPtr)->child[i]);
static_cast<gcllPtrT>(curPtr)->child[i] = new czllT;
goChild(i);
static_cast<czllPtrT>(curPtr)->clear();
curPtr->parent = _czllPtr;
static_cast<czllPtrT>(curPtr)->inheritCellPos(i);
const size_t newOct = getOctant(resPartPtr->pos);
static_cast<gcllPtrT>(curPtr)->child[newOct] = resPartPtr;
resPartPtr->depth++;
resPartPtr->parent = curPtr;
goUp();
}
else
if (not static_cast<gcllPtrT>(curPtr)->child[i]->isCZ)
{
const qcllPtrT oldCell =
static_cast<qcllPtrT>(static_cast<gcllPtrT>(curPtr)->child[i]);
static_cast<gcllPtrT>(curPtr)->child[i] = new czllT;
goChild(i);
static_cast<czllPtrT>(curPtr)->clear();
static_cast<czllPtrT>(curPtr)->initFromCell(*oldCell);
delete oldCell;
goUp();
}
}
///
/// now count the particles of this child
/// estimate the relative cost of those
///
const fType relCostPerPart = (_czllPtr->relCost / _czllPtr->noParts);
for (size_t i = 0; i < 8; i++)
{
goChild(i);
countParts = 0;
countPartsRecursor();
static_cast<czllPtrT>(curPtr)->relCost = countParts * relCostPerPart;
static_cast<czllPtrT>(curPtr)->noParts = countParts;
goUp();
}
///
/// now also distribute the orphans of the former parent cell to
/// the new bottom cells
///
nodePtrT curOrph = _czllPtr->orphFrst;
nodePtrT nxtOrph = NULL;
curPtr = _czllPtr;
while (curOrph != NULL)
{
// next ptr will be overwritten in adopt(), so we need a temporary
// storage for the next orphan
nxtOrph = curOrph->next;
const size_t newOct = getOctant(static_cast<pnodPtrT>(curOrph)->pos);
static_cast<czllPtrT>(_czllPtr->child[newOct])->adopt(
static_cast<pnodPtrT>(curOrph));
static_cast<czllPtrT>(_czllPtr->child[newOct])->noParts++;
static_cast<czllPtrT>(_czllPtr->child[newOct])->relCost += relCostPerPart;
curOrph = nxtOrph;
}
static_cast<czllPtrT>(curPtr)->orphFrst = NULL;
static_cast<czllPtrT>(curPtr)->orphLast = NULL;
}
void GL_ST7735::drawConicHelper(int xs, int ys, int xe, int ye, int color)
{
#if (DEBUG)
{
const char *fmt =
"GL_ST7735::drawConicHelper called with %7d %7d %7d %7d %7d" ;
char buf[snprintf(NULL, 0, fmt, xs, ys, xe, ye, color) + 1] ;
//
// note +1 for terminating null byte
//
snprintf(buf, sizeof buf, fmt, xs, ys, xe, ye, color) ;
Serial.println(buf) ;
}
#endif
#if (DEBUG)
{
const char *fmt =
"GL_ST7735::drawConicHelper -1- %12ld %12ld %12ld %12ld %12ld %12ld" ;
char buf[snprintf(NULL, 0, fmt, A,B,C,D,E,F) + 1] ;
//
// note +1 for terminating null byte
//
snprintf(buf, sizeof buf, fmt, A,B,C,D,E,F) ;
Serial.println(buf) ;
}
#endif
A *= 4;
B *= 4;
C *= 4;
D *= 4;
E *= 4;
F *= 4;
#if (DEBUG)
{
const char *fmt =
"GL_ST7735::drawConicHelper -2- %12ld %12ld %12ld %12ld %12ld %12ld" ;
char buf[snprintf(NULL, 0, fmt, A,B,C,D,E,F) + 1] ;
//
// note +1 for terminating null byte
//
snprintf(buf, sizeof buf, fmt, A,B,C,D,E,F) ;
Serial.println(buf) ;
}
#endif
// Translate start point to origin...
F = A*xs*xs + B*xs*ys + C*ys*ys + D*xs + E*ys + F;
D = D + 2 * A * xs + B * ys;
E = E + B * xs + 2 * C * ys;
// Work out starting octant
int octant = getOctant(D,E);
int dxS = SIDEx[octant];
int dyS = SIDEy[octant];
int dxD = DIAGx[octant];
int dyD = DIAGy[octant];
long d,u,v;
#if (DEBUG)
{
const char *fmt = "Before switch, octant = %d" ;
char buf[snprintf(NULL, 0, fmt, octant) + 1] ;
//
// note +1 for terminating null byte
//
snprintf(buf, sizeof buf, fmt, octant) ;
Serial.println(buf) ;
}
#endif
if (octant == 1) {
d = A + B / 2 + C / 4 + D + E / 2 + F;
u = A + B / 2 + D;
v = u + E;
} else if (octant == 2) {
d = A / 4 + B / 2 + C + D / 2 + E + F;
u = B / 2 + C + E;
v = u + D;
} else if (octant == 3) {
d = A / 4 - B / 2 + C - D / 2 + E + F;
u = -B / 2 + C + E;
v = u - D;
} else if (octant == 4) {
d = A - B / 2 + C / 4 - D + E / 2 + F;
u = A - B / 2 - D;
v = u + E;
} else if (octant == 5) {
d = A + B / 2 + C / 4 - D - E / 2 + F;
u = A + B / 2 - D;
v = u - E;
} else if (octant == 6) {
d = A / 4 + B / 2 + C - D / 2 - E + F;
u = B / 2 + C - E;
v = u - D;
} else if (octant == 7) {
d = A / 4 - B / 2 + C + D / 2 - E + F;
u = -B / 2 + C - E;
v = u + D;
} else if (octant == 8) {
d = A - B / 2 + C / 4 + D - E / 2 + F;
u = A - B / 2 + D;
v = u - E;
} else {
d=0 ; u=0 ; v=0 ;
const char *fmt = "FUNNY OCTANT";
char buf[snprintf(NULL, 0, fmt) + 1];
//
// note +1 for terminating null byte
//
snprintf(buf, sizeof buf, fmt);
Serial.println(buf);
while (true) {} ;
}
// switch (octant) {
// case 1:
// d = A + B / 2 + C / 4 + D + E / 2 + F;
// u = A + B / 2 + D;
// v = u + E;
// break;
// case 2:
// d = A / 4 + B / 2 + C + D / 2 + E + F;
// u = B / 2 + C + E;
// v = u + D;
// break;
// case 3:
// d = A / 4 - B / 2 + C - D / 2 + E + F;
// u = -B / 2 + C + E;
// v = u - D;
// break;
// case 4:
// d = A - B / 2 + C / 4 - D + E / 2 + F;
// u = A - B / 2 - D;
// v = u + E;
// break;
// case 5:
// d = A + B / 2 + C / 4 - D - E / 2 + F;
// u = A + B / 2 - D;
// v = u - E;
// break;
// case 6:
// d = A / 4 + B / 2 + C - D / 2 - E + F;
// u = B / 2 + C - E;
// v = u - D;
// break;
// case 7:
// d = A / 4 - B / 2 + C + D / 2 - E + F;
// u = -B / 2 + C - E;
// v = u + D;
// break;
// case 8:
// d = A - B / 2 + C / 4 + D - E / 2 + F;
// u = A - B / 2 + D;
// v = u - E;
// break;
// default:
// d=0 ; u=0 ; v=0 ;
// const char *fmt = "FUNNY OCTANT";
// char buf[snprintf(NULL, 0, fmt) + 1];
// //
// // note +1 for terminating null byte
// //
// snprintf(buf, sizeof buf, fmt);
// Serial.println(buf);
// while (true) {} ;
// break ;
// }
#if (DEBUG)
{
const char *fmt = "After switch, octant = %d" ;
char buf[snprintf(NULL, 0, fmt, octant) + 1] ;
//
// note +1 for terminating null byte
//
snprintf(buf, sizeof buf, fmt, octant) ;
Serial.println(buf) ;
}
#endif
long k1sign = dyS*dyD;
long k1 = 2 * (A + k1sign * (C - A));
long Bsign = dxD*dyD;
long k2 = k1 + Bsign * B;
long k3 = 2 * (A + C + Bsign * B);
// Work out gradient at endpoint
long gxe = xe - xs;
long gye = ye - ys;
long gx = 2*A*gxe + B*gye + D;
long gy = B*gxe + 2*C*gye + E;
int octantCount = getOctant(gx,gy) - octant;
if (octantCount < 0)
octantCount = octantCount + 8;
else if (octantCount==0)
if((xs>xe && dxD>0) || (ys>ye && dyD>0) ||
(xs<xe && dxD<0) || (ys<ye && dyD<0))
octantCount +=8;
#if (DEBUG)
{
const char *fmt = "octantCount = %d\n" ;
char buf[snprintf(NULL, 0, fmt, octantCount) + 1] ;
//
// note +1 for terminating null byte
//
snprintf(buf, sizeof buf, fmt, octantCount) ;
Serial.print(buf) ;
}
#endif
long x = xs;
long y = ys;
while (octantCount > 0) {
#if (DEBUG)
{
const char *fmt = "-- %d -------------------------\n" ;
char buf[snprintf(NULL, 0, fmt, octant) + 1] ;
//
// note +1 for terminating null byte
//
snprintf(buf, sizeof buf, fmt, octant) ;
Serial.print(buf) ;
}
#endif
if (odd(octant)) {
while (2*v <= k2) {
// Plot this point
drawPixel(x, y, color) ;
// Are we inside or outside?
#if (DEBUG)
{
const char *fmt = "x = %ld y = %ld d = %ld\n" ;
char buf[snprintf(NULL, 0, fmt, x,y,d) + 1] ;
//
// note +1 for terminating null byte
//
snprintf(buf, sizeof buf, fmt, x,y,d) ;
Serial.print(buf) ;
}
#endif
if (d < 0) { // Inside
x = x + dxS;
y = y + dyS;
u = u + k1;
v = v + k2;
d = d + u;
}
else { // outside
x = x + dxD;
y = y + dyD;
u = u + k2;
v = v + k3;
d = d + v;
}
}
d = d - u + v/2 - k2/2 + 3*k3/8;
// error (^) in Foley and van Dam p 959, "2nd ed, revised 5th printing"
u = -u + v - k2/2 + k3/2;
v = v - k2 + k3/2;
k1 = k1 - 2*k2 + k3;
k2 = k3 - k2;
int tmp = dxS; dxS = -dyS; dyS = tmp;
}
else { // Octant is even
while (2*u < k2) {
// Plot this point
drawPixel(x, y, color) ;
#if (DEBUG)
{
const char *fmt = "x = %ld y = %ld d = %ld\n" ;
char buf[snprintf(NULL, 0, fmt, x,y,d) + 1] ;
//
// note +1 for terminating null byte
//
snprintf(buf, sizeof buf, fmt, x,y,d) ;
Serial.print(buf) ;
}
#endif
// Are we inside or outside?
if (d > 0) { // Outside
x = x + dxS;
y = y + dyS;
u = u + k1;
v = v + k2;
d = d + u;
}
else { // Inside
x = x + dxD;
y = y + dyD;
u = u + k2;
v = v + k3;
d = d + v;
}
}
long tmpdk = k1 - k2;
d = d + u - v + tmpdk;
v = 2*u - v + tmpdk;
u = u + tmpdk;
k3 = k3 + 4*tmpdk;
k2 = k1 + tmpdk;
int tmp = dxD; dxD = -dyD; dyD = tmp;
}
octant = (octant&7)+1;
octantCount--;
}
// Draw final octant until we reach the endpoint
#if (DEBUG)
{
const char *fmt = "-- %d (final) -----------------\n" ;
char buf[snprintf(NULL, 0, fmt, octant) + 1] ;
//
// note +1 for terminating null byte
//
snprintf(buf, sizeof buf, fmt, octant) ;
Serial.print(buf) ;
}
#endif
if (odd(octant)) {
while (2*v <= k2) {
// Plot this point
drawPixel(x, y, color) ;
if (x == xe && y == ye)
break;
#if (DEBUG)
{
const char *fmt = "x = %ld y = %ld d = %ld\n" ;
char buf[snprintf(NULL, 0, fmt, x,y,d) + 1] ;
//
// note +1 for terminating null byte
//
snprintf(buf, sizeof buf, fmt, x,y,d) ;
Serial.print(buf) ;
}
#endif
// Are we inside or outside?
if (d < 0) { // Inside
x = x + dxS;
y = y + dyS;
u = u + k1;
v = v + k2;
d = d + u;
}
else { // outside
x = x + dxD;
y = y + dyD;
u = u + k2;
v = v + k3;
d = d + v;
}
}
}
else { // Octant is even
while ((2*u < k2)) {
// Plot this point
drawPixel(x, y, color) ;
if (x == xe && y == ye)
break;
#if (DEBUG)
{
const char *fmt = "x = %ld y = %ld d = %ld\n" ;
char buf[snprintf(NULL, 0, fmt, x,y,d) + 1] ;
//
// note +1 for terminating null byte
//
snprintf(buf, sizeof buf, fmt, x,y,d) ;
Serial.print(buf) ;
}
#endif
// Are we inside or outside?
if (d > 0) { // Outside
x = x + dxS;
y = y + dyS;
u = u + k1;
v = v + k2;
d = d + u;
}
else { // Inside
x = x + dxD;
y = y + dyD;
u = u + k2;
v = v + k3;
d = d + v;
}
}
}
}
