void CXRayObjectExport::buildEdgeTable( MDagPath& mesh )
{
//. if ( !smoothing ) return;
// Create our edge lookup table and initialize all entries to NULL
//
MFnMesh fnMesh( mesh );
edgeTableSize = fnMesh.numVertices();
edgeTable = xr_alloc<SXREdgeInfoPtr>(edgeTableSize);
ZeroMemory(edgeTable,edgeTableSize*sizeof(int));
// Add entries, for each edge, to the lookup table
//
MItMeshEdge eIt( mesh );
for ( ; !eIt.isDone(); eIt.next() )
{
bool smooth = eIt.isSmooth();
addEdgeInfo( eIt.index(0), eIt.index(1), smooth );
}
// Fill in referenced polygons
//
MItMeshPolygon pIt( mesh );
for ( ; !pIt.isDone(); pIt.next() )
{
int pvc = pIt.polygonVertexCount();
for ( int v=0; v<pvc; v++ )
{
int a = pIt.vertexIndex( v );
int b = pIt.vertexIndex( v==(pvc-1) ? 0 : v+1 );
SXREdgeInfoPtr elem = findEdgeInfo( a, b );
if ( NULL != elem ) {
int edgeId = pIt.index();
if ( INVALID_ID == elem->polyIds[0] ) {
elem->polyIds[0] = edgeId;
}
else {
elem->polyIds[1] = edgeId;
}
}
}
}
CreateSmoothingGroups( fnMesh );
}
void AddEdgeButton::addEdge() {
emit addEdgeInfo(fa, fb, hea, heb);
}
void MappingControllerThread(void) {
int16_t backLength; // the distance to the center of a crossing
int16_t w_right, w_left, w_front, w_rear; // the widths of a crossing
// needed for collecting us sensor data for the information about edges
int8_t us_pos = 0;
uint8_t wait_period = 1;
int8_t k = 1;
map_node_info node_info; // info about the crossing that shall be saved in the map
for (;;) {
// Seg_Hex(0x8);
os_wait(250);
// description of the algorithm for collecting this data:
// for every edge each 10 ( = NUMBER_US_VALUES) values of us left and right sensor data
// shall be saved. Therefore the array us_values is filled alternately with
// left (even positions) and right (odd positions) us sensor data. If the array is full,
// every second value will be deleted and afterwards only every second measured
// us sensor data is saved in the array to keep the distance between two values constant.
// and so on.
// --> The array will always be at least half-full and contain us sensor values with
// equal distances between them.
if (k == wait_period && getWidthFront()==0) {
// collect us values for edge information
if (us_pos < 2 * NUMBER_US_VALUES) {
us_values[us_pos] = Us_Data.Left_Distance; // even value: left
us_values[us_pos + 1] = Us_Data.Right_Distance; // odd value: right
us_pos += 2;
} else {
// take every second value and increment the wait period
// the array is half-full afterwards
uint8_t n;
for (n = 0; n < NUMBER_US_VALUES; n += 2) {
us_values[n] = us_values[2 * n];
us_values[n + 1] = us_values[2 * n + 1];
}
us_pos /= 2;
wait_period = wait_period<<=1; // wait_period * 2
}
k = 1; // reset k
} else
k++;
// ---------------------
// START of crossing
if (getCrossingStarted() == 1) {
uint8_t edge_length;
// calculate length of last edge
edge_length = calculateDrivenLen(os_getTime() - getTimeEdgeOld());
// add edge to map and increase the currentNodeNumber
addEdge(currentNodeNumber, ++currentNodeNumber, edge_length);
// reset value
resetCrossingStarted();
}
// ---------------------
// END of crossing
if (getCrossingDetected() == 0)
continue;
#ifdef MAPPINGCONTROLLER_DEBUG
printf("Crossing detected, MappingController is handling it...\r\n");
#endif
stopCrossingAnalyzer();
stopWallFollow();
// get the widths and add this information to the node info
w_left = getWidthLeft();
w_right = getWidthRight();
w_front = getWidthFront();
w_rear = getWidthRear();
node_info.w0 = w_front;
node_info.w1 = w_left;
node_info.w2 = w_rear;
node_info.w3 = w_right;
// Filter open doors etc.
// the width of a corridor must be bigger than 40cm
if (w_front < 40)
w_front = 0;
if (w_rear < 40)
w_rear = 0;
if (w_left < 40)
w_left = 0;
if (w_right < 40)
w_right = 0;
// Drive backwards to center of crossing
if (w_left != 0 && w_right != 0)
backLength = (w_right + w_left) / 4;
else
backLength = (w_right + w_left) / 2;
if (backLength == 0) // dead end
backLength = 20;
if (w_rear == 0)
backLength -= 20;
backLength *= -1; // backwards driving
#ifdef MAPPINGCONTROLLER_DEBUG
printf("Driving %dcm\r\n", backLength);
#endif
driveLen(backLength);
// Handle different crossing types
// width_front = direction the car came from
// width_rear = direction the car was heading to
// side_end: 1: coming up, 2: left, 3: straight, 4: right
// add information about the edge to the map and resume driving
if (w_left == 0 && w_right == 0 && w_front != 0 && w_rear == 0) {
noteCrossing(DeadEnd);
addEdgeInfo(last_edge_side, 1, NUMBER_US_VALUES, us_values);
last_edge_side = 1;
node_info.crossing_type = DeadEnd;
turn180(RIGHT, BACKWARDS);
resumeDriving(0);
} else if (w_left != 0 && w_right == 0 && w_front != 0 && w_rear != 0) {
noteCrossing(LeftStraight);
addEdgeInfo(last_edge_side, 3, NUMBER_US_VALUES, us_values);
last_edge_side = 3;
node_info.crossing_type = LeftStraight;
// Behavior: drive straight ahead!
resumeDriving(w_left / 2 + 50);
} else if (w_left == 0 && w_right != 0 && w_front != 0 && w_rear != 0) {
noteCrossing(RightStraight);
addEdgeInfo(last_edge_side, 4, NUMBER_US_VALUES, us_values);
last_edge_side = 4;
node_info.crossing_type = RightStraight;
turn90(RIGHT, BACKWARDS);
resumeDriving(w_rear / 2 + 50);
} else if (w_left == 0 && w_right != 0 && w_front != 0 && w_rear == 0) {
noteCrossing(RightOnly);
addEdgeInfo(last_edge_side, 4, NUMBER_US_VALUES, us_values);
last_edge_side = 4;
node_info.crossing_type = RightOnly;
turn90(RIGHT, BACKWARDS);
resumeDriving(w_rear / 2 + 50);
} else if (w_left != 0 && w_right == 0 && w_front != 0 && w_rear == 0) {
noteCrossing(LeftOnly);
addEdgeInfo(last_edge_side, 2, NUMBER_US_VALUES, us_values);
last_edge_side = 2;
node_info.crossing_type = LeftOnly;
turn90(LEFT, BACKWARDS);
resumeDriving(w_rear / 2 + 50);
} else if (w_left != 0 && w_right != 0 && w_front != 0 && w_rear == 0) {
noteCrossing(LeftRight);
addEdgeInfo(last_edge_side, 2, NUMBER_US_VALUES, us_values);
last_edge_side = 2;
node_info.crossing_type = LeftRight;
turn90(LEFT, BACKWARDS);
resumeDriving(w_rear / 2 + 50);
} else if (w_left != 0 && w_right != 0 && w_front != 0 && w_rear != 0) {
noteCrossing(All);
addEdgeInfo(last_edge_side, 3, NUMBER_US_VALUES, us_values);
last_edge_side = 3;
node_info.crossing_type = All;
// Behavior: drive straight ahead!
resumeDriving(w_left / 2 + 50);
} else { // dunno
resumeDriving(0);
}
// add information about the current node
addNodeInfo(currentNodeNumber, node_info);
// reset values
for (k = 0; k < 2 * NUMBER_US_VALUES; k++) {
us_values[k] = 0;
}
}
}
