// Checks all neighbours
void AstarExe::CheckNeighbours(Node * _node) {
//up
CheckNode(_node->column, _node->line - 1, _node->cost+1, _node);
//down
CheckNode(_node->column, _node->line + 1, _node->cost+1, _node);
//left
CheckNode(_node->column - 1, _node->line, _node->cost+1, _node);
//right
CheckNode(_node->column + 1, _node->line, _node->cost+1, _node);
}
int
MaxMem(int nprocs, int imrow, int imcol, int nmat, int *mval, int *nval, int nbmat, int *mbval,
int *nbval, int ngrids, int *npval, int *nqval, long *maxMem) {
int nprow, npcol, myrow, mycol;
int j, ierr[1];
long curMem;
*maxMem = 0;
for (j = 0; j < ngrids; ++j) {
nprow = npval[j];
npcol = nqval[j];
/* Make sure grid information is correct */
ierr[0] = 0;
if (nprow < 1) {
ierr[0] = 1;
} else if (npcol < 1) {
ierr[0] = 1;
} else if (nprow * npcol > nprocs) {
ierr[0] = 1;
}
if (ierr[0] > 0) {
continue;
}
for (myrow = 0; myrow < nprow; myrow++)
for (mycol = 0; mycol < npcol; mycol++) {
CheckNode( imrow, imcol, nmat, mval, nval, nbmat, mbval, nbval, myrow, mycol, nprow,
npcol, &curMem );
if (*maxMem < curMem) *maxMem = curMem;
}
}
return 0;
}
/*
FindNodeByIndex()
Cerca il nodo relativo all'indice.
*/
CNode* CNodeList::FindNodeByIndex(int index)
{
CNode* pNode = m_pFirstNode;
// scorre la lista
while(pNode!=(CNode*)NULL)
{
#ifdef _DEBUG
// controlla la signature
if(!CheckNode(pNode))
{
pNode = (CNode*)NULL;
break;
}
#endif
if(pNode->status==USED_NODE)
{
if(pNode->index==index)
break;
}
pNode = pNode->next;
}
return(pNode);
}
void OctreeBodyAttraction()
{
for (unsigned int i = 0; i < Bodies.size(); i++)
{
CheckNode(&GlobalNode, Bodies[i]);
}
}
int
main(int argc, char *argv[]) {
int n, nb, nprow, npcol, ng, lcm;
int nval[1], nbval[1];
long maxMem;
if (argc <= 1) {
printf( "Usage:\n%s n nb nprow npcol\n", argv[0] );
}
if (argc <= 1 || sscanf( argv[1], "%d", &n ) != 1 || n < 1) n = 50000;
if (argc <= 2 || sscanf( argv[2], "%d", &nb ) != 1 || nb < 1) nb = 80;
if (argc <= 3 || sscanf( argv[3], "%d", &nprow ) != 1 || nprow < 1) nprow = 8;
if (argc <= 4 || sscanf( argv[4], "%d", &npcol ) != 1 || npcol < 1) npcol = nprow;
nval[0] = n;
nbval[0] = nb;
CheckNode( 0, 0, 1, nval, nval, 1, nbval, nbval, 0, 0, nprow, npcol, &maxMem );
printf( "n=%d nb=%d nprow=%d npcol=%d lcm(nprow,npcol)=%d\n%ld\n", n, nb, nprow, npcol,
ilcm_(&nprow, &npcol), maxMem );
ng = iceil_(&n, &nb);
lcm = ilcm_(&nprow, &npcol);
printf( "%d %d %d\n", ng, lcm, (iceil_(&ng, &lcm) << 1) * nb * iceil_(&ng, &lcm) * nb );
printf( "%d %d\n", (iceil_(&ng, &lcm) << 1), iceil_(&ng, &lcm) );
return 0;
}
// /////////////////////
// Trace
// /////////////////////
TraceResult Trace(
const Bsp::CollisionBsp &bsp,
const Bounds &bounds)
{
// TODO: Deal with point tests (ray with length of 0).
// Find the maximum distance per axis from the bounds.
Vec3 extents =
{
std::abs(bounds.boxMin.data[0]) > std::abs(bounds.boxMax.data[0]) ?
std::abs(bounds.boxMin.data[0]) :
std::abs(bounds.boxMax.data[0]),
std::abs(bounds.boxMin.data[1]) > std::abs(bounds.boxMax.data[1]) ?
std::abs(bounds.boxMin.data[1]) :
std::abs(bounds.boxMax.data[1]),
std::abs(bounds.boxMin.data[2]) > std::abs(bounds.boxMax.data[2]) ?
std::abs(bounds.boxMin.data[2]) :
std::abs(bounds.boxMax.data[2]),
};
// Create an Axis Aligned Bounding Box (AABB)
// along the path of the trace, taking into
// consideration the sphere radius and the
// bounds extents.
auto aabbMin = Min(bounds.start, bounds.end);
aabbMin = aabbMin + -bounds.sphereRadius;
aabbMin = aabbMin - extents;
auto aabbMax = Max(bounds.start, bounds.end);
aabbMax = aabbMax + bounds.sphereRadius;
aabbMax = aabbMax + extents;
return CheckNode(
0,
0.0f,
1.0f,
bounds.start,
bounds.end,
extents,
{
bounds,
aabbMin,
aabbMax,
},
{
nullptr,
1.0f,
PathInfo::OutsideSolid
},
bsp);
}
CVector3 CQuake3BSP::Trace(CVector3 vStart, CVector3 vEnd)
{
// Initially we set our trace ratio to 1.0f, which means that we don't have
// a collision or intersection point, so we can move freely.
m_traceRatio = 1.0f;
// We start out with the first node (0), setting our start and end ratio to 0 and 1.
// We will recursively go through all of the nodes to see which brushes we should check.
CheckNode(0, 0.0f, 1.0f, vStart, vEnd);
// If the traceRatio is STILL 1.0f, then we never collided and just return our end position
if(m_traceRatio == 1.0f)
{
return vEnd;
}
else // Else COLLISION!!!!
{
// Set our new position to a position that is right up to the brush we collided with
CVector3 vNewPosition = vStart + ((vEnd - vStart) * m_traceRatio);
// Get the distance from the end point to the new position we just got
CVector3 vMove = vEnd - vNewPosition;
// Get the distance we need to travel backwards to the new slide position.
// This is the distance of course along the normal of the plane we collided with.
float distance = Dot(vMove, m_vCollisionNormal);
// Get the new end position that we will end up (the slide position).
CVector3 vEndPosition = vEnd - m_vCollisionNormal*distance;
// Since we got a new position for our sliding vector, we need to check
// to make sure that new sliding position doesn't collide with anything else.
vNewPosition = Trace(vNewPosition, vEndPosition);
/////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// *
//
if(m_vCollisionNormal.y > 0.2f || m_bGrounded)
m_bGrounded = true;
else
m_bGrounded = false;
/////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// *
// Return the new position to be used by our camera (or player)
return vNewPosition;
}
}
inline void CheckNode(Node* pNode, Body* pBody)
{
if (pNode->Bodies.size() != 0)
{
float diffX = (pNode->CenterOfMassx - pBody->posX);
float diffY = (pNode->CenterOfMassy - pBody->posY);
float distance = sqrt((diffX) * (diffX) + (diffY) * (diffY)); //Distance from the node to the object
if ((pNode->width / distance) < (_NODE_THRESHOLD) || (pNode->HasChildren == false)) //if sufficently far away or has no children (external node) group node and calculate force
{
CalculateForceNode(pBody, pNode, Softener);
pNode->IsUsed = true;
}
else //if not, repeat function with children
{
CheckNode(pNode->Child[0], pBody);
CheckNode(pNode->Child[1], pBody);
CheckNode(pNode->Child[2], pBody);
CheckNode(pNode->Child[3], pBody);
}
}
}
NODE* GetCircleNode(){
NODE * p = gHeadNode.pNext;
for(uint i = 1; i <= gLinkLength; ++i){
AddElem(p, p->hashVal);
if(p->pNext && CheckNode(p->pNext, p->pNext->hashVal)){
Destroy();
return p->pNext;
}
p = p->pNext;
}
Destroy();
return NULL;
}
void Collision::traceRay(vec3f start, vec3f end)
{
mInputStart = start;
mInputEnd = end;
mOutputFraction = 1.0f;
mGoodPos = true;
mTraceType = TT_RAY;
CheckNode( 0, 0.0f, 1.0f, mInputStart, mInputEnd );
if (mGoodPos)
{
mOutputEnd = mInputEnd;
}
else
{
mOutputEnd = mInputStart + mOutputFraction*(mInputEnd - mInputStart);
}
}
/*
EnumerateNodes()
Rinumera i nodi (occupati) presenti nella lista.
*/
void CNodeList::EnumerateNodes(void)
{
int i = 0;
CNode* pNode = m_pFirstNode;
// scorre la lista
while(pNode!=(CNode*)NULL)
{
#ifdef _DEBUG
// controlla la signature
if(!CheckNode(pNode))
break;
#endif
// controlla lo status
if(pNode->status==USED_NODE)
pNode->index = i++;
// passa al nodo successivo
pNode = pNode->next;
}
}
/*
ReleaseNodeList()
Rilascia la lista dei nodi.
*/
void CNodeList::ReleaseNodeList(int nMode)
{
CNode* pNode = m_pFirstNode;
CNode* pNextNode;
// scorre la lista
while(pNode!=(CNode*)NULL)
{
#ifdef _DEBUG
// controlla la signature
if(!CheckNode(pNode))
{
pNode = (CNode*)NULL;
break;
}
#endif
// salva l'indirizzo del nodo successivo
pNextNode = pNode->next;
// rilascia le risorse associate al nodo ed il nodo
ReleaseNode(pNode,nMode);
if(nMode==RELEASE_DELETE_MODE || nMode==RELEASE_ERASE_MODE)
delete pNode;
// passa al nodo successivo
if(nMode==RELEASE_DELETE_MODE || nMode==RELEASE_ERASE_MODE)
m_pFirstNode = pNode = pNextNode;
else
pNode = pNode->next;
}
// azzera il numero di elementi presenti
if(nMode==RELEASE_DELETE_MODE || nMode==RELEASE_ERASE_MODE)
m_nTot = 0;
// resetta la lista
if(nMode==RELEASE_DELETE_MODE || nMode==RELEASE_ERASE_MODE)
m_pFirstNode = m_pLastNode = (CNode*)NULL;
}
BOOL
MT::CheckNode (GiSTpath path, MTentry *parentEntry)
{
MTnode *node = (MTnode *) ReadNode (path);
BOOL ret = TRUE;
for (int i=0; i<node->NumEntries() && ret; i++) {
MTentry *nextEntry = (MTentry *) (*node)[i].Ptr();
if (parentEntry!=NULL && (nextEntry->Key()->distance+nextEntry->MaxRadius() > parentEntry->MaxRadius() || nextEntry->Key()->distance != nextEntry->object().distance(parentEntry->object()))) {
cout << "Error with entry " << nextEntry << "in " << node;
ret = FALSE;
}
if (!node->IsLeaf()) {
path.MakeChild (nextEntry->Ptr());
ret &= CheckNode (path, nextEntry);
path.MakeParent ();
}
}
delete node;
return ret;
}
/*
CountNodes()
Conta i nodi (occupati) presenti nella lista.
*/
int CNodeList::CountNodes(void)
{
int tot = 0;
CNode* pNode = m_pFirstNode;
// scorre la lista
while(pNode!=(CNode*)NULL)
{
#ifdef _DEBUG
// controlla la signature
if(!CheckNode(pNode))
break;
#endif
// controlla lo status ed incrementa il totale dei nodi occupati
if(pNode->status==USED_NODE)
tot++;
// passa al nodo successivo
pNode = pNode->next;
}
return(tot);
}
void Collision::trace()
{
int numTries = 0;
while (numTries < 3)
{
mOutputFraction = 1.0f;
mGoodPos = true;
CheckNode( 0, 0.0f, 1.0f, mInputStart, mInputEnd );
if (mGoodPos)
{
mOutputEnd = mInputEnd;
return;
}
else
{
mInputEnd = mInputEnd + mCollisionNormal*(-mEndDistance + EPSILON);
numTries++;
}
}
// did not find a good position
mOutputEnd = mInputStart;
}
/*
FindNextNode()
Restituisce il puntatore al successivo nodo (occupato) della lista.
Passare il puntatore al corrente, restituisce il puntatore al seguente.
*/
CNode* CNodeList::FindNextNode(CNode* pNode)
{
// scorre la lista (a partire dall'elemento corrente, ossia quello ricevuto come parametro)
while(pNode!=(CNode*)NULL)
{
#ifdef _DEBUG
// controlla la signature
if(!CheckNode(pNode))
{
pNode = (CNode*)NULL;
break;
}
#endif
// posiziona sul nodo seguente (il puntatore sta' puntando al nodo corrente del chiamante)
if((pNode = pNode->next)!=(CNode*)NULL)
{
// controlla lo status
if(pNode->status==USED_NODE)
break;
}
}
return(pNode);
}
/*
FindFirstNode()
Cerca il primo nodo della lista con lo status uguale a quello specificato.
*/
CNode* CNodeList::FindFirstNode(int status)
{
CNode* pNode = m_pFirstNode;
// scorre la lista
while(pNode!=(CNode*)NULL)
{
#ifdef _DEBUG
// controlla la signature
if(!CheckNode(pNode))
{
pNode = (CNode*)NULL;
break;
}
#endif
// se lo status e' diverso da quello specificato, passa al nodo successivo
if(pNode->status!=status)
pNode = pNode->next;
else
break;
}
return(pNode);
}
void CHeap1::Free( void* mem ) {
Node* Tofree = (Node*)( (char*)mem - sizeof(Node) );
mem = NULL;
if ( !CheckNode( Tofree ) ) {
// This is not a vilide node, something wrong
return;
}
// because sentinel is always exsit as a free node, so any node in freelist will at least has one p*Free != NULL
#define ISFREE( node ) ( CheckNode( node ) && node->pPrevFree != NULL || node->pNextFree != NULL )
Node* Prev = Tofree->pPrevMem;
Node* Next = Tofree->pNextMem;
Node* node = Tofree;
if ( Prev == pSentinel ) {
if ( ISFREE( Next ) ) {
node->pPrevFree = Next->pPrevFree;
node->pNextFree = Next->pNextFree;
Next->pPrevFree->pNextFree = node;
if ( Next->pNextFree ) Next->pNextFree->pPrevFree = node;
node->pNextMem = Next->pNextMem;
if ( Next->pNextMem <= pHeapEnd ) node->pNextMem->pPrevMem = node;
}
else {
Tofree->pNextFree = pSentinel->pNextFree;
Tofree->pPrevFree = pSentinel;
if ( pSentinel->pNextFree ) pSentinel->pNextFree->pPrevFree = Tofree;
pSentinel->pNextFree = Tofree;
}
return;
}
if ( ISFREE( Prev ) && ISFREE( Next ) ) {
// Prev and Next both in FreeList
Next->pPrevFree->pNextFree = Next->pNextFree;
if ( Next->pNextFree ) Next->pNextFree->pPrevFree = Next->pPrevFree;
Prev->pNextMem = Next->pNextMem;
if ( Next->pNextMem <= pHeapEnd ) Next->pNextMem->pPrevMem = Prev;
}
else if ( ISFREE( Prev ) ) {
Prev->pNextMem = node->pNextMem;
if ( node->pNextMem <= pHeapEnd ) node->pNextMem->pPrevMem = Prev;
}
else if ( ISFREE( Next ) ) {
node->pPrevFree = Next->pPrevFree;
node->pNextFree = Next->pNextFree;
Next->pPrevFree->pNextFree = node;
if ( Next->pNextFree ) Next->pNextFree->pPrevFree = node;
node->pNextMem = Next->pNextMem;
if ( Next->pNextMem <= pHeapEnd ) node->pNextMem->pPrevMem = node;
}
else {
// merge failed
Tofree->pNextFree = pSentinel->pNextFree;
Tofree->pPrevFree = pSentinel;
if ( pSentinel->pNextFree ) pSentinel->pNextFree->pPrevFree = Tofree;
pSentinel->pNextFree = Tofree;
}
return;
#undef ISFREE
}
int main(int argc, char const *argv[])
{
// rng stuff
std::random_device rd; // device entropy
std::mt19937 m_rng(rd()); // initialize our mersenne twister with a random seed
std::uniform_real_distribution<double> double_gen(1e-300,38);
// ---------------------------------- check correctness of checkNodes computations in VariableNode --------------------------
for(int a = 0; a < 111; ++a) {
for(int b = 0; b < 293; ++b) {
VariableNode node(a, b);
for(int i = 0; i < 7; ++i) { // a*slope + c
if( ((a*slopes[node.getCheckNodes()[i].first] + node.getCheckNodes()[i].second)%293 != b) || node.getCheckNodes()[i].second < 0) {
if (!(node.getCheckNodes()[i].second < 0 && reverseModulo((int)ALL_COLUMNS, a*slopes[i], b) == 292)) { // c = 292 -> c = -1
std::cout << "a = " << a << " b = " << b << " s_index = " << node.getCheckNodes()[i].first << " c = " << node.getCheckNodes()[i].second << "\n";
}
}
}
}
}
// ------------------------------------------- test phiTilde function for large values ----------------------------------------
for(int i = 30; i < 40; ++i) {
std::cout << "i = " << i << " phiTilde = " << phiTilde(i) << " isNaN? " << isnan(phiTilde(i)) << "\n";
}
// ----------------------------------------- test phiTilde function for very small values -------------------------------------
for(int i = 300; i < 320; i++) {
std::cout << "10^{-" << i << "} = " << std::pow(10, -i) << " phiTilde = " << phiTilde(std::pow(10, -i)) << "\n";
}
// ------------------------------------------------------ test infinity sum ---------------------------------------------------
std::cout << "Expecting inf -> " << phiTilde(std::pow(10, -400)) + 2 << "\n";
std::cout << "Expecting -inf -> " << -phiTilde(std::pow(10, -400)) + 2 << "\n";
std::cout << "Expecting 2 -> " << phiTilde(phiTilde(std::pow(10, -400))) + 2 << "\n";
// ---------------------------------------------------- phiTilde performances -------------------------------------------------
const int N = 1e6;
// create N random values
std::array<double, N> random_array;
for(int i = 0; i < N; ++i) {
random_array[i] = double_gen(m_rng);
}
std::chrono::time_point<std::chrono::system_clock> begin = std::chrono::system_clock::now();
double val;
for(int i = 0; i < N; ++i) {
val = phiTilde(random_array[i]);
}
std::chrono::nanoseconds duration = std::chrono::system_clock::now() - begin;
std::cout << "Average time to compute phiTilde(x) " << (double)duration.count()/N << " ns\n";
// ----------------------------------------- test the update LLR function on variableNode --------------------------------------
int a = 1;
int b = 0;
VariableNode node(a, b);
for(int i = 0; i < 7; ++i) {
std::cout << slopes[node.getCheckNodes()[i].first] << " c " << node.getCheckNodes()[i].second << "\n";
}
// set channel LLR and propagate it
node.setChannelLLR(6);
for(int i = 0; i < PC_ROWS + 1; ++i) {
std::cout << "LLR = " << node.getLLRat(i) << "\n";
}
// create checkNodesVector
std::vector<CheckNode> *checkNodesVector = new std::vector<CheckNode>(2051);
for(int slope_index = 0; slope_index < (int)PC_ROWS - 1; ++slope_index) { // init the first six blocks
for(int c = 0; c < (int)ALL_COLUMNS - 1; ++c) { // with 292 valid nodes each
checkNodesVector->at(slope_index*(int)ALL_COLUMNS + c).setLine(line(slope_index, c));
}
}
// init the last block
for(int c = 0; c < (int)ALL_COLUMNS; ++c) { // with 293 valid nodes
checkNodesVector->at(((int)PC_ROWS - 1)*(int)ALL_COLUMNS + c).setLine(line((int)PC_ROWS - 1, c));
}
// fill the LLR of the blocks for (1,0) -> 0+293, 293+291, 293*2+290, 293*3+289, 293*4+288, 293*5 + 287, 293*6+286, each at row 1
checkNodesVector->at(293).setLLRat(3, 1); // this will not be summed, since this check node is not valid
checkNodesVector->at(293+291).setLLRat(3, 1);
checkNodesVector->at(293*2+290).setLLRat(2, 1);
checkNodesVector->at(293*3+289).setLLRat(-4, 1);
checkNodesVector->at(293*4+288).setLLRat(-1, 1);
checkNodesVector->at(293*5+287).setLLRat(3, 1);
checkNodesVector->at(293*6+286).setLLRat(0, 1);
node.updateLLR(checkNodesVector);
std::cout << "LLR = " << node.getLLRat(0) << " expecting 9\n";
std::cout << "LLR = " << node.getLLRat(1) << " expecting 6\n";
std::cout << "LLR = " << node.getLLRat(2) << " expecting 7\n";
std::cout << "LLR = " << node.getLLRat(3) << " expecting 13\n";
std::cout << "LLR = " << node.getLLRat(4) << " expecting 10\n";
std::cout << "LLR = " << node.getLLRat(5) << " expecting 6\n";
std::cout << "LLR = " << node.getLLRat(6) << " expecting 9\n";
std::cout << "LLR = " << node.getLLRat(7) << " expecting 3\n";
delete checkNodesVector;
// ----------------------------------------- test the update LLR function on checkNode ----------------------------------------
// Create VariableNode vector
std::vector<VariableNode> *variableNodeVector = new std::vector<VariableNode>;
variableNodeVector->resize(ALL_COLUMNS*ALL_ROWS);
// Init its coordinates
for(int node_index = 0; node_index < variableNodeVector->size(); ++node_index) {
// row // column
variableNodeVector->at(node_index).setCoordinates(node_index/(int)ALL_COLUMNS, node_index%(int)ALL_COLUMNS);
}
int line_index = 2;
int node_c = 3;
CheckNode check_node(line(line_index,node_c));
double all_llr = -11;
// init LLR on the corresponding nodes
for(int a = 0; a < (int)ALL_ROWS; ++a) {
variableNodeVector->at(a*ALL_COLUMNS + (slopes[line_index]*a + node_c)%ALL_COLUMNS).setLLRat(all_llr, line_index);
}
// set an LLR on a different branch to 0 and check that it does not change the results
variableNodeVector->at(106*ALL_COLUMNS + (slopes[line_index]*106 + node_c)%ALL_COLUMNS).setLLRat(0, line_index+1);
double phiTildeNode = phiTilde(std::abs(all_llr));
double exp_llr = (all_llr > 0) ? phiTilde(111*phiTildeNode) : -phiTilde(111*phiTildeNode);
begin = std::chrono::system_clock::now();
check_node.updateLLR(variableNodeVector);
std::cout << "updateLLR time " << (double)(std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now() - begin)).count()/1000 << " ms\n";
std::cout << "Expecting " << exp_llr << " computed " << check_node.getLLRat(1) << " " << check_node.getLLRat(2) <<"\n";
variableNodeVector->at(110*ALL_COLUMNS + (slopes[line_index]*110 + node_c)%ALL_COLUMNS).setLLRat(-all_llr, line_index);
phiTildeNode = phiTilde(std::abs(all_llr));
exp_llr = (all_llr > 0) ? -phiTilde(111*phiTildeNode) : phiTilde(111*phiTildeNode);
begin = std::chrono::system_clock::now();
check_node.updateLLR(variableNodeVector);
std::cout << "updateLLR time " << (double)(std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now() - begin)).count()/1000 << " ms\n";
std::cout << "Expecting " << exp_llr << " computed " << check_node.getLLRat(1) << " " << check_node.getLLRat(2) <<"\n";
std::cout << "For node 110, expecting " << -exp_llr << " computed " << check_node.getLLRat(110) <<"\n";
variableNodeVector->at(107*ALL_COLUMNS + (slopes[line_index]*107 + node_c)%ALL_COLUMNS).setLLRat(-all_llr, line_index);
phiTildeNode = phiTilde(std::abs(all_llr));
exp_llr = (all_llr > 0) ? phiTilde(111*phiTildeNode) : -phiTilde(111*phiTildeNode);
check_node.updateLLR(variableNodeVector);
std::cout << "Expecting " << exp_llr << " computed " << check_node.getLLRat(1) << " " << check_node.getLLRat(2) <<"\n";
std::cout << "For node 110, expecting " << -exp_llr << " computed " << check_node.getLLRat(110) <<"\n";
std::cout << "For node 107, expecting " << -exp_llr << " computed " << check_node.getLLRat(107) <<"\n";
// set one LLR to 0 -> also the resulting llr should be 0
variableNodeVector->at(106*ALL_COLUMNS + (slopes[line_index]*106 + node_c)%ALL_COLUMNS).setLLRat(0, line_index);
phiTildeNode = phiTilde(std::abs(all_llr));
exp_llr = (all_llr > 0) ? phiTilde(111*phiTildeNode) : -phiTilde(111*phiTildeNode);
check_node.updateLLR(variableNodeVector);
std::cout << "Expecting " << 0 << " computed " << check_node.getLLRat(1) << " " << check_node.getLLRat(2) <<"\n";
std::cout << "For node 110, expecting " << 0 << " computed " << check_node.getLLRat(110) <<"\n";
std::cout << "For node 107, expecting " << 0 << " computed " << check_node.getLLRat(107) <<"\n";
std::cout << "For node 106, expecting " << exp_llr << " computed " << check_node.getLLRat(106) <<"\n";
// set one LLR to infinity -> the result is given only by the other ones
variableNodeVector->at(106*ALL_COLUMNS + (slopes[line_index]*106 + node_c)%ALL_COLUMNS).setLLRat(std::numeric_limits<double>::infinity(), line_index);
phiTildeNode = phiTilde(std::abs(all_llr));
exp_llr = phiTilde(110*phiTildeNode);
check_node.updateLLR(variableNodeVector);
std::cout << "Expecting " << exp_llr << " computed " << check_node.getLLRat(1) << " " << check_node.getLLRat(2) <<"\n";
std::cout << "For node 110, expecting " << -exp_llr << " computed " << check_node.getLLRat(110) <<"\n";
std::cout << "For node 107, expecting " << -exp_llr << " computed " << check_node.getLLRat(107) <<"\n";
std::cout << "For node 106, expecting " << ((all_llr > 0) ? phiTilde(111*phiTildeNode) : -phiTilde(111*phiTildeNode)) << " computed " << check_node.getLLRat(106) <<"\n";
for(int a = 0; a < (int)ALL_ROWS; ++a) {
variableNodeVector->at(a*ALL_COLUMNS + (slopes[line_index]*a + node_c)%ALL_COLUMNS).setLLRat(std::numeric_limits<double>::infinity(), line_index);
}
phiTildeNode = phiTilde(std::abs(std::numeric_limits<double>::infinity()));
exp_llr = (std::numeric_limits<double>::infinity() > 0) ? phiTilde(111*phiTildeNode) : -phiTilde(111*phiTildeNode);
check_node.updateLLR(variableNodeVector);
std::cout << "Expecting " << exp_llr << " computed " << check_node.getLLRat(1) << " " << check_node.getLLRat(2) <<"\n";
// ----------------------------------------- test the min sum update LLR function on checkNode ----------------------------------------
std::cout << "Min Sum test\n";
line_index = 2;
node_c = 3;
CheckNode check_node_ms = CheckNode(line(line_index,node_c));
all_llr = -11;
// init LLR on the corresponding nodes
for(int a = 0; a < (int)ALL_ROWS; ++a) {
variableNodeVector->at(a*ALL_COLUMNS + (slopes[line_index]*a + node_c)%ALL_COLUMNS).setLLRat(all_llr, line_index);
}
// set an LLR on a different branch to 0 and check that it does not change the results
variableNodeVector->at(106*ALL_COLUMNS + (slopes[line_index]*106 + node_c)%ALL_COLUMNS).setLLRat(0, line_index+1);
begin = std::chrono::system_clock::now();
check_node_ms.updateLLRminSum(variableNodeVector);
std::cout << "updateLLR time " << (double)(std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now() - begin)).count()/1000 << " ms\n";
std::cout << "Expecting " << all_llr << " computed " << check_node_ms.getLLRat(1) << " " << check_node_ms.getLLRat(2) <<"\n";
variableNodeVector->at(110*ALL_COLUMNS + (slopes[line_index]*110 + node_c)%ALL_COLUMNS).setLLRat(-all_llr, line_index);
exp_llr = -all_llr;
begin = std::chrono::system_clock::now();
check_node_ms.updateLLRminSum(variableNodeVector);
std::cout << "updateLLR time " << (double)(std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now() - begin)).count()/1000 << " ms\n";
std::cout << "Expecting " << exp_llr << " computed " << check_node_ms.getLLRat(1) << " " << check_node_ms.getLLRat(2) <<"\n";
std::cout << "For node 110, expecting " << -exp_llr << " computed " << check_node_ms.getLLRat(110) <<"\n";
variableNodeVector->at(107*ALL_COLUMNS + (slopes[line_index]*107 + node_c)%ALL_COLUMNS).setLLRat((all_llr > 0 ? -(all_llr - 1) : -(all_llr + 1)), line_index);
exp_llr = (all_llr > 0) ? all_llr-1 : -(all_llr-1);
check_node_ms.updateLLRminSum(variableNodeVector);
std::cout << "Expecting " << (all_llr > 0 ? (all_llr - 1) : (all_llr + 1)) << " computed " << check_node_ms.getLLRat(1) << " " << check_node_ms.getLLRat(2) <<"\n";
std::cout << "For node 110, expecting " << (all_llr < 0 ? -(all_llr + 1) : -(all_llr - 1)) << " computed " << check_node_ms.getLLRat(110) <<"\n";
std::cout << "For node 107, expecting " << -all_llr << " computed " << check_node_ms.getLLRat(107) <<"\n";
// set one LLR to 0 -> also the resulting llr should be 0
variableNodeVector->at(106*ALL_COLUMNS + (slopes[line_index]*106 + node_c)%ALL_COLUMNS).setLLRat(0, line_index);
check_node_ms.updateLLRminSum(variableNodeVector);
std::cout << "Expecting " << 0 << " computed " << check_node_ms.getLLRat(1) << " " << check_node_ms.getLLRat(2) <<"\n";
std::cout << "For node 110, expecting " << 0 << " computed " << check_node_ms.getLLRat(110) <<"\n";
std::cout << "For node 107, expecting " << 0 << " computed " << check_node_ms.getLLRat(107) <<"\n";
std::cout << "For node 106, expecting " << (all_llr > 0 ? (all_llr - 1) : (all_llr + 1)) << " computed " << check_node_ms.getLLRat(106) <<"\n";
// set one LLR to infinity -> the result is given only by the other ones
variableNodeVector->at(106*ALL_COLUMNS + (slopes[line_index]*106 + node_c)%ALL_COLUMNS).setLLRat(std::numeric_limits<double>::infinity(), line_index);
check_node_ms.updateLLRminSum(variableNodeVector);
std::cout << "Expecting " << (all_llr > 0 ? (all_llr - 1) : -(all_llr + 1)) << " computed " << check_node_ms.getLLRat(1) << " " << check_node_ms.getLLRat(2) <<"\n";
std::cout << "For node 110, expecting " << -(all_llr > 0 ? (all_llr - 1) : -(all_llr + 1)) << " computed " << check_node_ms.getLLRat(110) <<"\n";
std::cout << "For node 107, expecting " << (all_llr > 0 ? -all_llr : all_llr) << " computed " << check_node_ms.getLLRat(107) <<"\n";
std::cout << "For node 106, expecting " << (all_llr > 0 ? (all_llr - 1) : (all_llr + 1)) << " computed " << check_node_ms.getLLRat(106) <<"\n";
for(int a = 0; a < (int)ALL_ROWS; ++a) {
variableNodeVector->at(a*ALL_COLUMNS + (slopes[line_index]*a + node_c)%ALL_COLUMNS).setLLRat(std::numeric_limits<double>::infinity(), line_index);
}
check_node_ms.updateLLRminSum(variableNodeVector);
std::cout << "Expecting " << std::numeric_limits<double>::infinity() << " computed " << check_node_ms.getLLRat(1) << " " << check_node_ms.getLLRat(2) <<"\n";
return 0;
}
void Collision::CheckNode( int nodeIndex, float startFraction, float endFraction, vec3f start, vec3f end)
{
if (nodeIndex < 0)
{
// this is a leaf
Q3BspLeaf *leaf = &mQ3Map->m_pLeafs[-(nodeIndex + 1)];
for (int i = 0; i < leaf->n_leafbrushes; i++)
{
Q3BspBrush *brush = &mQ3Map->m_pBrushes[mQ3Map->m_pLeafBrushes[leaf->leafbrush + i]];
if (brush->n_brushsides > 0 && (mQ3Map->m_pTextures[brush->texture].contents & 1) )
{
CheckBrush( brush );
}
}
// don't have to do anything else for leaves
return;
}
// this is a node
Q3BspNode *node = &mQ3Map->m_pNodes[nodeIndex];
Q3BspPlane *plane = &mQ3Map->m_pPlanes[node->plane];
vec3f normal = vec3f(plane->normal);
vec3f startDX = vec3f(start);
vec3f endDX = vec3f(end);
float startDistance = vec3dot( &startDX, &normal ) - plane->dist;
float endDistance = vec3dot( &endDX, &normal ) - plane->dist;
float offset;
if (mTraceType == TT_RAY)
{
offset = 0.0f;
}
else if (mTraceType == TT_SPHERE)
{
offset = mTraceRadius;
}
else
{
offset = 0.0f;
}
if (startDistance >= offset && endDistance >= offset)
{ // both points are in front of the plane
// so check the front child
CheckNode( node->children[0], startFraction, endFraction, start, end );
}
else if (startDistance < -offset && endDistance < -offset)
{ // both points are behind the plane
// so check the back child
CheckNode( node->children[1], startFraction, endFraction, start, end );
}
else
{ // the line spans the splitting plane
int side;
float fraction1, fraction2, middleFraction;
vec3f middle;
// split the segment into two
if (startDistance < endDistance)
{
side = 1; // back
float inverseDistance = 1.0f / (startDistance - endDistance);
fraction1 = (startDistance - offset + EPSILON) * inverseDistance;
fraction2 = (startDistance + offset + EPSILON) * inverseDistance;
}
else if (endDistance < startDistance)
{
side = 0; // front
float inverseDistance = 1.0f / (startDistance - endDistance);
fraction1 = (startDistance + offset + EPSILON) * inverseDistance;
fraction2 = (startDistance - offset - EPSILON) * inverseDistance;
}
else
{
side = 0; // front
fraction1 = 1.0f;
fraction2 = 0.0f;
}
// make sure the numbers are valid
if (fraction1 < 0.0f) fraction1 = 0.0f;
else if (fraction1 > 1.0f) fraction1 = 1.0f;
if (fraction2 < 0.0f) fraction2 = 0.0f;
else if (fraction2 > 1.0f) fraction2 = 1.0f;
// calculate the middle point for the first side
middleFraction = startFraction + (endFraction - startFraction) * fraction1;
/*for (int i = 0; i < 3; i++)
middle[i] = start[i] + fraction1 * (end[i] - start[i]);*/
middle = start + fraction1 * (end - start);
// check the first side
CheckNode( node->children[side], startFraction, middleFraction, start, middle );
// calculate the middle point for the second side
middleFraction = startFraction + (endFraction - startFraction) * fraction2;
/*for (int i = 0; i < 3; i++)
middle[i] = start[i] + fraction2 * (end[i] - start[i]);*/
middle = start + fraction2 * (end - start);
// check the second side
CheckNode( node->children[!side], middleFraction, endFraction, middle, end );
}
}
void CQuake3BSP::CheckNode(int nodeIndex, float startRatio, float endRatio, CVector3 vStart, CVector3 vEnd)
{
// Check if the next node is a leaf
if(nodeIndex < 0)
{
// If this node in the BSP is a leaf, we need to negate and add 1 to offset
// the real node index into the m_pLeafs[] array. You could also do [~nodeIndex].
tBSPLeaf *pLeaf = &m_pLeafs[-(nodeIndex + 1)];
// We have a leaf, so let's go through all of the brushes for that leaf
for(int i = 0; i < pLeaf->numOfLeafBrushes; i++)
{
// Get the current brush that we going to check
tBSPBrush *pBrush = &m_pBrushes[m_pLeafBrushes[pLeaf->leafBrush + i]];
// Check if we have brush sides and the current brush is solid and collidable
if((pBrush->numOfBrushSides > 0) && (m_pTextures[pBrush->textureID].textureType & 1))
{
// Now we delve into the dark depths of the real calculations for collision.
// We can now check the movement vector against our brush planes.
CheckBrush(pBrush, vStart, vEnd);
}
}
// Since we found the brushes, we can go back up and stop recursing at this level
return;
}
// Grad the next node to work with and grab this node's plane data
tBSPNode *pNode = &m_pNodes[nodeIndex];
tBSPPlane *pPlane = &m_pPlanes[pNode->plane];
// Here we use the plane equation to find out where our initial start position is
// according the the node that we are checking. We then grab the same info for the end pos.
float startDistance = Dot(vStart, pPlane->vNormal) - pPlane->d;
float endDistance = Dot(vEnd, pPlane->vNormal) - pPlane->d;
float offset = 0.0f;
// If we are doing sphere collision, include an offset for our collision tests below
if(m_traceType == TYPE_SPHERE)
offset = m_traceRadius;
// Here we check to see if we are working with a BOX or not
else if(m_traceType == TYPE_BOX)
{
// Get the distance our AABB is from the current splitter plane
offset = (float)(fabs( m_vExtents.x * pPlane->vNormal.x ) +
fabs( m_vExtents.y * pPlane->vNormal.y ) +
fabs( m_vExtents.z * pPlane->vNormal.z ) );
}
// Here we check to see if the start and end point are both in front of the current node.
// If so, we want to check all of the nodes in front of this current splitter plane.
if(startDistance >= offset && endDistance >= offset)
{
// Traverse the BSP tree on all the nodes in front of this current splitter plane
CheckNode(pNode->front, startDistance, endDistance, vStart, vEnd);
}
// If both points are behind the current splitter plane, traverse down the back nodes
else if(startDistance < -offset && endDistance < -offset)
{
// Traverse the BSP tree on all the nodes in back of this current splitter plane
CheckNode(pNode->back, startDistance, endDistance, vStart, vEnd);
}
else
{
// If we get here, then our ray needs to be split in half to check the nodes
// on both sides of the current splitter plane. Thus we create 2 ratios.
float Ratio1 = 1.0f, Ratio2 = 0.0f, middleRatio = 0.0f;
CVector3 vMiddle; // This stores the middle point for our split ray
// Start of the side as the front side to check
int side = pNode->front;
// Here we check to see if the start point is in back of the plane (negative)
if(startDistance < endDistance)
{
// Since the start position is in back, let's check the back nodes
side = pNode->back;
// Here we create 2 ratios that hold a distance from the start to the
// extent closest to the start (take into account a sphere and epsilon).
float inverseDistance = 1.0f / (startDistance - endDistance);
Ratio1 = (startDistance - offset - kEpsilon) * inverseDistance;
Ratio2 = (startDistance + offset + kEpsilon) * inverseDistance;
}
// Check if the starting point is greater than the end point (positive)
else if(startDistance > endDistance)
{
// This means that we are going to recurse down the front nodes first.
// We do the same thing as above and get 2 ratios for split ray.
float inverseDistance = 1.0f / (startDistance - endDistance);
Ratio1 = (startDistance + offset + kEpsilon) * inverseDistance;
Ratio2 = (startDistance - offset - kEpsilon) * inverseDistance;
}
// Make sure that we have valid numbers and not some weird float problems.
// This ensures that we have a value from 0 to 1 as a good ratio should be :)
if (Ratio1 < 0.0f) Ratio1 = 0.0f;
else if (Ratio1 > 1.0f) Ratio1 = 1.0f;
if (Ratio2 < 0.0f) Ratio2 = 0.0f;
else if (Ratio2 > 1.0f) Ratio2 = 1.0f;
// Just like we do in the Trace() function, we find the desired middle
// point on the ray, but instead of a point we get a middleRatio percentage.
middleRatio = startRatio + ((endRatio - startRatio) * Ratio1);
vMiddle = vStart + ((vEnd - vStart) * Ratio1);
// Now we recurse on the current side with only the first half of the ray
CheckNode(side, startRatio, middleRatio, vStart, vMiddle);
// Now we need to make a middle point and ratio for the other side of the node
middleRatio = startRatio + ((endRatio - startRatio) * Ratio2);
vMiddle = vStart + ((vEnd - vStart) * Ratio2);
// Depending on which side should go last, traverse the bsp with the
// other side of the split ray (movement vector).
if(side == pNode->back)
CheckNode(pNode->front, middleRatio, endRatio, vMiddle, vEnd);
else
CheckNode(pNode->back, middleRatio, endRatio, vMiddle, vEnd);
}
}
static int FindNextLeafNode( BTScanState *scanState, Boolean avoidIO )
{
int err;
err = noErr; // Assume everything will be OK
while ( 1 )
{
if ( scanState->nodesLeftInBuffer == 0 )
{
// Time to read some more nodes into the buffer
if ( avoidIO )
{
return fsBTTimeOutErr;
}
else
{
// read some more nodes into buffer
err = ReadMultipleNodes( scanState );
if ( err != noErr )
break;
}
}
else
{
// Adjust the node counters and point to the next node in the buffer
++scanState->nodeNum;
--scanState->nodesLeftInBuffer;
// If we've looked at all nodes in the tree, then we're done
if ( scanState->nodeNum >= scanState->btcb->totalNodes )
return fsEndOfIterationErr;
if ( scanState->nodesLeftInBuffer == 0 )
{
scanState->recordNum = 0;
continue;
}
(u_int8_t *) scanState->currentNodePtr += scanState->btcb->nodeSize;
}
#if BYTE_ORDER == LITTLE_ENDIAN
{
BlockDescriptor block;
FileReference fref;
/* Fake a BlockDescriptor */
block.buffer = scanState->currentNodePtr;
block.blockSize = scanState->btcb->nodeSize;
block.blockReadFromDisk = 1;
block.isModified = 0;
fref = scanState->btcb->fileRefNum;
SWAP_BT_NODE(&block, ISHFSPLUS(VTOVCB(fref)), VTOC(fref)->c_fileid, 0);
}
#endif
// Make sure this is a valid node
if ( CheckNode( scanState->btcb, scanState->currentNodePtr ) != noErr )
{
continue;
}
if ( scanState->currentNodePtr->kind == kBTLeafNode )
break;
}
return err;
} /* FindNextLeafNode */
void Run(berry::IWorkbenchPartSite::Pointer workbenchPartSite, mitk::DataStorage::Pointer dataStorage, const QList<mitk::DataNode::Pointer>& selectedNodes /*= QList<mitk::DataNode::Pointer>()*/, mitk::BaseRenderer* baseRenderer /*= nullptr*/)
{
if (selectedNodes.empty())
{
return;
}
auto renderWindow = mitk::WorkbenchUtil::GetRenderWindowPart(workbenchPartSite->GetPage(), mitk::WorkbenchUtil::NONE);
if (nullptr == renderWindow)
{
renderWindow = mitk::WorkbenchUtil::OpenRenderWindowPart(workbenchPartSite->GetPage(), false);
if (nullptr == renderWindow)
{
// no render window available
return;
}
}
auto boundingBoxPredicate = mitk::NodePredicateNot::New(mitk::NodePredicateProperty::New("includeInBoundingBox", mitk::BoolProperty::New(false), baseRenderer));
mitk::DataStorage::SetOfObjects::Pointer nodes = mitk::DataStorage::SetOfObjects::New();
for (const auto& dataNode : selectedNodes)
{
if (boundingBoxPredicate->CheckNode(dataNode))
{
nodes->InsertElement(nodes->Size(), dataNode);
}
}
if (nodes->empty())
{
return;
}
if (1 == nodes->Size()) // Special case: If exactly one ...
{
auto image = dynamic_cast<mitk::Image*>(nodes->ElementAt(0)->GetData());
if (nullptr != image) // ... image is selected, reinit is expected to rectify askew images.
{
if (nullptr == baseRenderer)
{
mitk::RenderingManager::GetInstance()->InitializeViews(image->GetTimeGeometry(), mitk::RenderingManager::REQUEST_UPDATE_ALL, true);
}
else
{
mitk::RenderingManager::GetInstance()->InitializeView(baseRenderer->GetRenderWindow(), image->GetTimeGeometry(), true);
}
return;
}
}
auto boundingGeometry = dataStorage->ComputeBoundingGeometry3D(nodes, "visible", baseRenderer);
if (nullptr == baseRenderer)
{
mitk::RenderingManager::GetInstance()->InitializeViews(boundingGeometry);
}
else
{
mitk::RenderingManager::GetInstance()->InitializeView(baseRenderer->GetRenderWindow(), boundingGeometry);
}
}
void Pathfinder::GeneratePath()
{
sLog.Write("Generating new path");
// Destroy old path
Path = std::stack<Vector2i>();
// Zero out status grid
std::memset(PathfindingStatusGrid.get(), 0, MAX_MAP_HEIGHT * MAX_MAP_WIDTH * sizeof(uint8));
// Get Node with same position as creature in pathfinding grid
PathfinderNode* pOriginNode = &(PathfindingGrid[MAX_MAP_HEIGHT * pOrigin->GetY() + pOrigin->GetX()]);
// Origin node does not have parent node
pOriginNode->pParent = nullptr;
// Origin node does not have G cost
pOriginNode->G = 0;
pOriginNode->H = 10 * math::GetManhattanDistance(Vector2i(pOriginNode->Position.x, pOriginNode->Position.y), Target);
// Add origin node to queue
OpenList.push(pOriginNode);
do
{
// Grab node with highest priority
PathfinderNode* pCurrent = OpenList.top();
OpenList.pop();
// Move it to "closed" list
PathfindingStatusGrid[MAX_MAP_HEIGHT * pCurrent->Position.y + pCurrent->Position.x] = CLOSED;
// If current node has same coordinates as target, we found our path
if(pCurrent->Position == Target)
{
// Create path using parents to track down origin
while(pCurrent->pParent != nullptr)
{
Path.push(pCurrent->Position);
pCurrent = pCurrent->pParent;
}
// Destroy open list
OpenList.clear();
return;
}
// For each of adjacent nodes
// if it is not out of bounds
// and if tile is 'walkable',
// check its status
// Upper
if(pCurrent->Position.y != 0)
{
CheckNode(pCurrent, 0, -1, 10);
}
// Lower
if(pCurrent->Position.y != pTileGrid->size()-1)
{
CheckNode(pCurrent, 0, 1, 10);
}
// Right
if(pCurrent->Position.x != (*pTileGrid)[0].size()-1)
{
CheckNode(pCurrent, 1, 0, 10);
}
// Left
if(pCurrent->Position.x != 0)
{
CheckNode(pCurrent, -1, 0, 10);
}
// Upper-right
if(pCurrent->Position.x != (*pTileGrid)[0].size()-1 && pCurrent->Position.y != 0)
{
CheckNode(pCurrent, 1, -1, 14);
}
// Upper-left
if(pCurrent->Position.x != 0 && pCurrent->Position.y != 0)
{
CheckNode(pCurrent, -1, -1, 14);
}
// Lower-right
if(pCurrent->Position.x != (*pTileGrid)[0].size()-1 && pCurrent->Position.y != pTileGrid->size()-1)
{
CheckNode(pCurrent, 1, 1, 14);
}
// Lower-left
if(pCurrent->Position.x != 0 && pCurrent->Position.y != pTileGrid->size()-1)
{
CheckNode(pCurrent, -1, 1, 14);
}
} while(!OpenList.empty());
sLog.Write("Could not generate path");
}
TraceResult CheckNode(
int nodeIndex,
float startFraction,
float endFraction,
const Vec3& start,
const Vec3& end,
const Vec3& extents,
const TraceBounds& boundsAabb,
TraceResult result,
const Bsp::CollisionBsp& bsp)
{
if (result.pathFraction <= startFraction)
{
// already hit something nearer
return result;
}
if (nodeIndex < 0)
{
// this is a leaf
const auto& leaf = bsp.leaves[-(nodeIndex + 1)];
for (int i = 0; i < leaf.leafBrushCount; i++)
{
const auto& brush =
bsp.brushes[bsp.leafBrushes[leaf.firstLeafBrushIndex + i].brushIndex];
// Don't even bother if there are no brush sides.
if (brush.brush.sideCount <= 0)
{
continue;
}
// Only test solid brushes
// 1 == CONTENTS_SOLID
if (!(bsp.textures[brush.brush.textureIndex].contentFlags & 1))
{
continue;
}
// Early exit if the AABB doesn't collide.
if (AabbDontIntersect(
boundsAabb.aabbMin,
boundsAabb.aabbMax,
brush.aabbMin,
brush.aabbMax))
{
continue;
}
result = CheckBrush(bsp, brush.brush, boundsAabb.bounds, result);
}
// don't have to do anything else for leaves
return result;
}
// this is a node
const auto& node = bsp.nodes[nodeIndex];
const auto& plane = bsp.planes[node.planeIndex];
float startDistance = DotF(start, plane.normal) - plane.distance;
float endDistance = DotF(end, plane.normal) - plane.distance;
// Offset used for non-ray tests.
const auto& bounds = boundsAabb.bounds;
float offset = bounds.sphereRadius;
// extents are zero for ray or sphere tests.
offset +=
std::abs(extents.data[0] * plane.normal.data[0]) +
std::abs(extents.data[1] * plane.normal.data[1]) +
std::abs(extents.data[2] * plane.normal.data[2]);
if (startDistance >= offset && endDistance >= offset)
{
// both points are in front of the plane
// so check the front child
return CheckNode(
node.childIndex[0],
startFraction,
endFraction,
start,
end,
extents,
boundsAabb,
result,
bsp);
}
if (startDistance < -offset && endDistance < -offset)
{
// both points are behind the plane
// so check the back child
return CheckNode(
node.childIndex[1],
startFraction,
endFraction,
start,
end,
extents,
boundsAabb,
result,
bsp);
}
// the line spans the splitting plane
// Default values assume startDistance == endDistance.
int side = 0;
float fraction1 = 1.0f;
float fraction2 = 0.0f;
// split the segment into two
if (startDistance < endDistance)
{
// back
side = 1;
float inverseDistance = 1.0f / (startDistance - endDistance);
fraction1 = (startDistance - offset + EPSILON) * inverseDistance;
fraction2 = (startDistance + offset + EPSILON) * inverseDistance;
}
if (endDistance < startDistance)
{
// front
float inverseDistance = 1.0f / (startDistance - endDistance);
fraction1 = (startDistance + offset + EPSILON) * inverseDistance;
fraction2 = (startDistance - offset - EPSILON) * inverseDistance;
}
// make sure the numbers are valid
fraction1 = Clamp0To1(fraction1);
fraction2 = Clamp0To1(fraction2);
// calculate the middle point for the first side
{
auto middleFraction =
startFraction + (endFraction - startFraction) * fraction1;
auto middle = Lerp(start, end, fraction1);
// check the first side
result = CheckNode(
node.childIndex[side],
startFraction,
middleFraction,
start,
middle,
extents,
boundsAabb,
result,
bsp);
}
// calculate the middle point for the second side
{
auto middleFraction =
startFraction + (endFraction - startFraction) * fraction2;
auto middle = Lerp(start, end, fraction2);
// check the second side
result = CheckNode(
node.childIndex[!side],
middleFraction,
endFraction,
middle,
end,
extents,
boundsAabb,
result,
bsp);
}
return result;
}
