int main(void)
{
int w;
int i;
assert((win = bunny_start(800, 600, false, "The Lapins Noirs")) != NULL);
assert((pic[0] = bunny_new_picture(NORM(800, 600), NORM(800, 600))) != NULL);
assert((pic[1] = bunny_new_picture(NORM(800, 600), NORM(800, 600))) != NULL);
assert((bunny = bunny_load_picture("bigbunny.png")) != NULL);
bunny->origin.x = bunny->buffer.width / 2;
bunny->origin.y = bunny->buffer.height / 2;
pic[0]->origin.x = pic[0]->buffer.width / 2;
pic[0]->origin.y = pic[0]->buffer.height / 2;
pic[1]->origin.x = pic[1]->buffer.width / 2;
pic[1]->origin.y = pic[1]->buffer.height / 2;
bunny_clear(&pic[0]->buffer, 0);
bunny_clear(&pic[1]->buffer, 0);
for (i = 0, w = 50; i < pic[0]->buffer.width; i += 100)
{
square(pic[0], i, 0, w, pic[0]->buffer.height, ALPHA(150, rand()));
square(pic[1], i, 0, w, pic[1]->buffer.height, ALPHA(150, rand()));
}
reset(pic[0]);
reset(pic[1]);
bunny_set_loop_main_function(loop);
bunny_set_key_response(key);
bunny_loop(win, 50, NULL);
bunny_delete_clipable(pic[0]);
bunny_delete_clipable(pic[1]);
bunny_stop(win);
return (EXIT_FAILURE);
}
void Screen::mouseMoveEvent(QMouseEvent *event) {
if (!m_moving && m_conns->mouseMoveEvent(event)) {
ScreenObject *object = getObject(event->x(), event->y());
if (!object) {
return;
}
int pin = getPin(object, event->x(), event->y());
if (pin == -1) {
return;
}
Pin &p = object->getPins()[pin];
QToolTip::showText(mapToGlobal(event->pos()), p.name);
return;
}
if (event->buttons() & Qt::LeftButton) {
if (m_moving) {
m_moving->setX(NORM(m_moving->x() - (m_movingX - event->x())));
m_moving->setY(NORM(m_moving->y() - (m_movingY - event->y())));
m_conns->movePins(m_moving);
resizeAccordingToObjects();
repaint();
}
else {
ScreenObject *object = getObject(event->x(), event->y());
if (!object) {
return;
}
int pin = getPin(object, event->x(), event->y());
if (pin != -1) {
return;
}
m_moving = object;
}
m_movingX = NORM(event->x());
m_movingY = NORM(event->y());
}
else {
if (m_moving) {
m_conns->objectMoved(m_moving);
}
m_moving = 0;
ScreenObject *object = getObject(event->x(), event->y());
if (!object) {
return;
}
int pin = getPin(object, event->x(), event->y());
if (pin == -1) {
return;
}
Pin &p = object->getPins()[pin];
QToolTip::showText(mapToGlobal(event->pos()), p.name);
}
}
void
make_rotation(size_t n, double **rot, double *x, double *y, int use_alpha, double alpha)
{
double nx, ny, xy, c, s;
size_t i, j;
for (i = 0; i < n; i++) {
for (j = 0; j < n; j++) rot[i][j] = 0.0;
rot[i][i] = 1.0;
}
nx = NORM(n, x);
ny = NORM(n, y);
if (nx == 0.0 || ny == 0.0) return;
for (i = 0; i < n; i++) x[i] /= nx;
for (i = 0; i < n; i++) y[i] /= ny;
xy = inner_product(n, x, y);
c = (use_alpha) ? cos(alpha) : xy;
s = (use_alpha) ? sin(alpha) : sqrt(1 - c * c);
for (i = 0; i < n; i++) y[i] -= xy * x[i];
ny = NORM(n, y);
if (ny == 0.0) return;
for (i = 0; i < n; i++) y[i] /= ny;
for (i = 0; i < n; i++) {
for (j = 0; j < n; j++)
rot[i][j] = x[j] * ( x[i] * (c - 1.0) + y[i] * s)
+ y[j] * (- x[i] * s + y[i] * (c - 1.0));
rot[i][i] += 1.0;
}
}
int address_table_lookup(address_table_t* const table,
const uint32_t ip_address,
const uint8_t mac[ETH_ALEN]) {
if (!is_address_private(ip_address)) {
return -1;
}
if (table->length > 0) {
/* Search table starting w/ most recent MAC addresses. */
int idx;
for (idx = 0; idx < table->length; ++idx) {
int mac_id = NORM(table->last - idx);
if (table->entries[mac_id].ip_address == ip_address
&& !memcmp(table->entries[mac_id].mac_address, mac, ETH_ALEN)) {
return mac_id;
}
}
}
if (table->length == MAC_TABLE_ENTRIES) {
/* Discard the oldest MAC address. */
table->first = NORM(table->first + 1);
} else {
++table->length;
}
if (table->length > 1) {
table->last = NORM(table->last + 1);
}
table->entries[table->last].ip_address = ip_address;
memcpy(table->entries[table->last].mac_address, mac, ETH_ALEN);
if (table->added_since_last_update < MAC_TABLE_ENTRIES) {
++table->added_since_last_update;
}
return table->last;
}
void shNgin3dUpright()
{
g_Ngin3d_uprightMode = true;
float targetDir[3] = {_Ngin3d_cameraTarget[0]-_Ngin3d_cameraPosition[0], _Ngin3d_cameraTarget[1]-_Ngin3d_cameraPosition[1], _Ngin3d_cameraTarget[2]-_Ngin3d_cameraPosition[2]};
if ((_Ngin3d_cameraUp[1] < SIN_1 && targetDir[1] > 0) || (_Ngin3d_cameraUp[1] > -SIN_1 && targetDir[1] < 0))
{
// latitude minus 1 degree
targetDir[0] -= _Ngin3d_cameraUp[0]*TAN_1;
targetDir[1] -= _Ngin3d_cameraUp[1]*TAN_1;
targetDir[2] -= _Ngin3d_cameraUp[2]*TAN_1;
float norm = NORM(targetDir);
targetDir[0] /= norm;
targetDir[1] /= norm;
targetDir[2] /= norm;
}
else if ((_Ngin3d_cameraUp[1] < SIN_1 && targetDir[1] < 0) || (_Ngin3d_cameraUp[1] > -SIN_1 && targetDir[1] > 0))
{
// latitude plus 1 degree
targetDir[0] += _Ngin3d_cameraUp[0]*TAN_1;
targetDir[1] += _Ngin3d_cameraUp[1]*TAN_1;
targetDir[2] += _Ngin3d_cameraUp[2]*TAN_1;
float norm = NORM(targetDir);
targetDir[0] /= norm;
targetDir[1] /= norm;
targetDir[2] /= norm;
}
float lat = asin(targetDir[1]);
if (_Ngin3d_cameraUp[1] < 0)
lat = PI-lat;
float lon = asin(targetDir[0]/cos(lat)); // cos(lat) can't be zero because targetDir has already been shifted to outside �� if ((targetDir[2] < 0 && cos(lat) > 0) || (targetDir[2] > 0 && cos(lat) < 0)) // actually: targetDir[2]/cos(lat) < 0
lon = PI-lon;
_Ngin3d_latitude = lat*180/PI;
_Ngin3d_longitude = lon*180/PI;
}
void MyQGLWidget::paintGL()
{
#define NORM(x) ((float)x / 255.f)
glClearColor(NORM(84), NORM(124), NORM(196), 1);
Director::sharedDirector()->drawScene();
MySceneEditor::instance()->drawOverlay();
}
bool ConnectionManager::mouseMoveEvent(QMouseEvent *event) {
if (m_fromPin != -1) {
// qDebug() << NORM(event->x()) << NORM(event->y());
m_screen->repaint();
return true;
}
if (event->buttons() & Qt::LeftButton) {
if (m_moving) {
m_moving->setX(m_moving->x() - (m_movingX - NORM(event->x())));
m_moving->setY(m_moving->y() - (m_movingY - NORM(event->y())));
m_movingX = NORM(event->x());
m_movingY = NORM(event->y());
m_screen->repaint();
return true;
}
else {
// int point;
// Connection *c = getPoint(NORM(event->x()), NORM(event->y()), point);
// if (point != -1) {
// m_moving = &c->points[point];
// }
// else {
// QPointF intersectPnt;
// int point;
// c = getConnection(NORM(event->x()), NORM(event->y()), point, &intersectPnt);
// if (c) {
// m_fromObject = 0;
// m_movingConn = c;
// QPoint p = intersectPnt.toPoint();
// c->points.insert(c->points.begin() + point, p);
// m_moving = &(c->points[point]);
// }
// }
}
m_movingX = NORM(event->x());
m_movingY = NORM(event->y());
}
else {
int point;
Connection *c = getPoint(NORM(event->x()), NORM(event->y()), point);
if (point != -1) {
return true;
}
else {
QPointF intersectPnt;
int point;
c = getConnection(NORM(event->x()), NORM(event->y()), point, &intersectPnt);
if (c) {
return true;
}
}
m_moving = 0;
m_fromObject = 0;
m_movingConn = 0;
}
return false;
}
void fmpz_add(fmpz_t coeffs_out, const fmpz_t in1, const fmpz_t in2)
{
fmpz_t coeffs1 = in1;
fmpz_t coeffs2 = in2;
long carry;
unsigned long size1 = ABS(coeffs1[0]);
unsigned long size2 = ABS(coeffs2[0]);
if (size1 < size2)
{
SWAP_PTRS(coeffs1, coeffs2);
size1 = ABS(coeffs1[0]);
size2 = ABS(coeffs2[0]);
}
if (!size1)
{
if (!size2) coeffs_out[0] = 0L;
else
{
if (coeffs_out != coeffs2) F_mpn_copy(coeffs_out, coeffs2, size2+1);
}
} else if (!size2)
{
if (coeffs_out != coeffs1) F_mpn_copy(coeffs_out, coeffs1, size1+1);
} else if ((long) (coeffs1[0] ^ coeffs2[0]) >= 0L)
{
coeffs_out[0] = coeffs1[0];
carry = mpn_add(coeffs_out+1, coeffs1+1, size1, coeffs2+1, size2);
if (carry)
{
coeffs_out[size1+1] = carry;
if ((long) coeffs_out[0] < 0L) coeffs_out[0]--;
else coeffs_out[0]++;
}
} else
{
carry = 0;
if (size1 != size2) carry = 1;
else carry = mpn_cmp(coeffs1+1, coeffs2+1, size1);
if (carry == 0) coeffs_out[0] = 0L;
else if (carry > 0)
{
mpn_sub(coeffs_out+1, coeffs1+1, size1, coeffs2+1, size2);
coeffs_out[0] = coeffs1[0];
NORM(coeffs_out);
}
else
{
mpn_sub_n(coeffs_out+1, coeffs2+1, coeffs1+1, size1);
coeffs_out[0] = -coeffs1[0];
NORM(coeffs_out);
}
}
}
double Area(int n, Point *poly)
{
int i;
double area = 0;
for (i = 0; i < n; i++)
area += poly[NORM(i, n)].x * poly[NORM(i + 1, n)].y - poly[NORM(i + 1, n)].x * poly[NORM(i, n)].y;
return fabs(area) / 2;
}
void Screen::addObject(const QPoint &pos) {
AddPeripheral dialog(m_peripherals, this);
if (dialog.exec() == QDialog::Accepted) {
QString name = dialog.getPeripheral();
Peripheral *p = m_peripherals->getPeripheral(name).create();
QPoint local = mapFromGlobal(pos);
p->setX(NORM(local.x()));
p->setY(NORM(local.y()));
addObject(p);
}
}
void ConnectionManager::addConnectionNode(Connection *c, int point, int x, int y, std::vector<QPoint> &points) {
ConnectionNode *node = new ConnectionNode();
node->setX(NORM(x) - node->width()/2);
node->setY(NORM(y) - node->height()/2);
std::vector<QPoint> newPoints;
newPoints.push_back(QPoint(NORM(x), NORM(y)));
while (c->points.size() != point + 1) {
qDebug() << c->points[point + 1];
newPoints.push_back(c->points[point + 1]);
c->points.erase(c->points.begin() + point + 1);
}
m_screen->addObject(node);
int addpin1 = 0;
int addpin2 = 0;
int addpin3 = 0;
#define PINPOS(POINT, VAR)\
qDebug() << abs(POINT.y() - NORM(y)) << abs(POINT.x() - NORM(x));\
if (abs(POINT.y() - NORM(y)) < abs(POINT.x() - NORM(x))) { \
if (POINT.x() < NORM(x)) \
VAR = 0; \
else \
VAR = 2; \
} \
else { \
if (POINT.y() < NORM(y)) \
VAR = 3; \
else \
VAR = 1; \
} \
qDebug() << VAR;
qDebug() << newPoints[0] << newPoints[1] << newPoints[newPoints.size() - 1];
PINPOS(c->points[c->points.size() - 2], addpin1);
PINPOS(newPoints[newPoints.size() - 1], addpin2);
PINPOS(points[points.size() - 2], addpin3);
Connection *c2 = addConnection(node, addpin2, c->to, c->tpin, newPoints);
node->setConnection(addpin2, c2);
c->to = node;
c->tpin = addpin1;
movePins(c->to);
movePins(c->from);
node->setConnection(addpin1, c);
Connection *c3 = addConnection(m_fromObject, m_fromPin, node, addpin3, points);
node->setConnection(addpin3, c3);
}
int address_table_write_update(address_table_t* const table, gzFile handle) {
if (!gzprintf(handle,
"%d %d\n",
NORM(table->last - table->added_since_last_update + 1),
MAC_TABLE_ENTRIES)) {
#ifndef NDEBUG
perror("Error writing update");
#endif
return -1;
}
int idx;
for (idx = table->added_since_last_update; idx > 0; --idx) {
int mac_id = NORM(table->last - idx + 1);
#ifndef DISABLE_ANONYMIZATION
uint64_t digest_ip;
uint8_t digest_mac[ETH_ALEN];
if (anonymize_ip(table->entries[mac_id].ip_address, &digest_ip)
|| anonymize_mac(table->entries[mac_id].mac_address, digest_mac)) {
#ifndef NDEBUG
fprintf(stderr, "Error anonymizing MAC mapping\n");
#endif
return -1;
}
if (!gzprintf(handle,
"%s %" PRIx64 "\n",
buffer_to_hex(digest_mac, ETH_ALEN),
digest_ip)) {
#else
if (!gzprintf(handle,
"%s %" PRIx32 "\n",
buffer_to_hex(table->entries[mac_id].mac_address, ETH_ALEN),
table->entries[mac_id].ip_address)) {
#endif
#ifndef NDEBUG
perror("Error writing update");
#endif
return -1;
}
}
if (!gzprintf(handle, "\n")) {
#ifndef NDEBUG
perror("Error writing update");
#endif
return -1;
}
table->added_since_last_update = 0;
return 0;
}
void shNginFrustum::shNginFCreateFrustum(float *frontBottomLeft, float *backTopRight, float *n1, float *n2, float *n3, float *n4, float *n5, float *n6)
{
vertices[NGIN_FRUSTUM_FRONT_BOTTOM_LEFT][0] = frontBottomLeft[0];
vertices[NGIN_FRUSTUM_FRONT_BOTTOM_LEFT][1] = frontBottomLeft[1];
vertices[NGIN_FRUSTUM_FRONT_BOTTOM_LEFT][2] = frontBottomLeft[2];
vertices[NGIN_FRUSTUM_BACK_TOP_RIGHT][0] = backTopRight[0];
vertices[NGIN_FRUSTUM_BACK_TOP_RIGHT][1] = backTopRight[1];
vertices[NGIN_FRUSTUM_BACK_TOP_RIGHT][2] = backTopRight[2];
normals[NGIN_FRUSTUM_FRONT][0] = n1[0]; normals[NGIN_FRUSTUM_FRONT][1] = n1[1]; normals[NGIN_FRUSTUM_FRONT][2] = n1[2];
normals[NGIN_FRUSTUM_BOTTOM][0] = n2[0]; normals[NGIN_FRUSTUM_BOTTOM][1] = n2[1]; normals[NGIN_FRUSTUM_BOTTOM][2] = n2[2];
normals[NGIN_FRUSTUM_LEFT][0] = n3[0]; normals[NGIN_FRUSTUM_LEFT][1] = n3[1]; normals[NGIN_FRUSTUM_LEFT][2] = n3[2];
normals[NGIN_FRUSTUM_BACK][0] = n4[0]; normals[NGIN_FRUSTUM_BACK][1] = n4[1]; normals[NGIN_FRUSTUM_BACK][2] = n4[2];
normals[NGIN_FRUSTUM_TOP][0] = n5[0]; normals[NGIN_FRUSTUM_TOP][1] = n5[1]; normals[NGIN_FRUSTUM_TOP][2] = n5[2];
normals[NGIN_FRUSTUM_RIGHT][0] = n6[0]; normals[NGIN_FRUSTUM_RIGHT][1] = n6[1]; normals[NGIN_FRUSTUM_RIGHT][2] = n6[2];
for (int i = 0; i < 6; ++i)
{
float norm = NORM(normals[i]);
if (norm < EPSILON)
{
normals[i][0] = 0;
normals[i][1] = 0;
normals[i][2] = 1;
}
else
{
normals[i][0] /= norm;
normals[i][1] /= norm;
normals[i][2] /= norm;
}
}
// Now, find the rest of the points. They are the points were three planes of different sides collide.
float RHS[6] = {DOT(normals[NGIN_FRUSTUM_FRONT], vertices[NGIN_FRUSTUM_FRONT_BOTTOM_LEFT]),
DOT(normals[NGIN_FRUSTUM_BOTTOM], vertices[NGIN_FRUSTUM_FRONT_BOTTOM_LEFT]),
DOT(normals[NGIN_FRUSTUM_LEFT], vertices[NGIN_FRUSTUM_FRONT_BOTTOM_LEFT]),
DOT(normals[NGIN_FRUSTUM_BACK], vertices[NGIN_FRUSTUM_BACK_TOP_RIGHT]),
DOT(normals[NGIN_FRUSTUM_TOP], vertices[NGIN_FRUSTUM_BACK_TOP_RIGHT]),
DOT(normals[NGIN_FRUSTUM_RIGHT], vertices[NGIN_FRUSTUM_BACK_TOP_RIGHT])};
float det = 0;
#define DET(x11,x12,x13,x21,x22,x23,x31,x32,x33) ((x11)*(x22)*(x33)+(x12)*(x23)*(x31)+(x13)*(x21)*(x32)-(x11)*(x23)*(x32)-(x12)*(x21)*(x33)-(x13)*(x22)*(x31))
#define SOLVE_VERTEX(side1, side2, side3) \
det = DET(normals[NGIN_FRUSTUM_##side1][0], normals[NGIN_FRUSTUM_##side1][1], normals[NGIN_FRUSTUM_##side1][2],\
normals[NGIN_FRUSTUM_##side2][0], normals[NGIN_FRUSTUM_##side2][1], normals[NGIN_FRUSTUM_##side2][2],\
normals[NGIN_FRUSTUM_##side3][0], normals[NGIN_FRUSTUM_##side3][1], normals[NGIN_FRUSTUM_##side3][2]);\
if (det < EPSILON) { *this = shNginFrustum(); return; }\
vertices[NGIN_FRUSTUM_##side1##_##side2##_##side3][0] = DET(RHS[NGIN_FRUSTUM_##side1], normals[NGIN_FRUSTUM_##side1][1], normals[NGIN_FRUSTUM_##side1][2],\
RHS[NGIN_FRUSTUM_##side2], normals[NGIN_FRUSTUM_##side2][1], normals[NGIN_FRUSTUM_##side2][2],\
RHS[NGIN_FRUSTUM_##side3], normals[NGIN_FRUSTUM_##side3][1], normals[NGIN_FRUSTUM_##side3][2])/det;\
vertices[NGIN_FRUSTUM_##side1##_##side2##_##side3][1] = DET(normals[NGIN_FRUSTUM_##side1][0], RHS[NGIN_FRUSTUM_##side1], normals[NGIN_FRUSTUM_##side1][2],\
normals[NGIN_FRUSTUM_##side2][0], RHS[NGIN_FRUSTUM_##side2], normals[NGIN_FRUSTUM_##side2][2],\
normals[NGIN_FRUSTUM_##side3][0], RHS[NGIN_FRUSTUM_##side3], normals[NGIN_FRUSTUM_##side3][2])/det;\
vertices[NGIN_FRUSTUM_##side1##_##side2##_##side3][2] = DET(normals[NGIN_FRUSTUM_##side1][0], normals[NGIN_FRUSTUM_##side1][1], RHS[NGIN_FRUSTUM_##side1],\
normals[NGIN_FRUSTUM_##side2][0], normals[NGIN_FRUSTUM_##side2][1], RHS[NGIN_FRUSTUM_##side2],\
normals[NGIN_FRUSTUM_##side3][0], normals[NGIN_FRUSTUM_##side3][1], RHS[NGIN_FRUSTUM_##side3])/det
SOLVE_VERTEX(FRONT, BOTTOM, RIGHT);
SOLVE_VERTEX(FRONT, TOP, RIGHT);
SOLVE_VERTEX(FRONT, TOP, LEFT);
SOLVE_VERTEX(BACK, BOTTOM, LEFT);
SOLVE_VERTEX(BACK, BOTTOM, RIGHT);
SOLVE_VERTEX(BACK, TOP, LEFT);
}
void fmpz_tdiv(fmpz_t res, const fmpz_t a, const fmpz_t b)
{
long a0 = a[0];
long b0 = b[0];
unsigned long sizea = FLINT_ABS(a0);
unsigned long sizeb = FLINT_ABS(b0);
while ((!a[sizea]) && (sizea)) sizea--;
while ((!b[sizeb]) && (sizeb)) sizeb--;
mp_limb_t mslimb;
fmpz_t temp;
if (sizeb == 0)
{
printf("Error: division by zero!\n");
abort();
} else if (sizea < sizeb) // Todo: make this deal with sizea == sizeb but a < b
{
res[0] = 0;
} else
{
temp = (fmpz_t) flint_stack_alloc(sizeb);
mpn_tdiv_qr(res+1, temp, 0, a+1, sizea, b+1, sizeb);
res[0] = sizea - sizeb + 1;
if ((long) (a0 ^ b0) < 0) res[0] = -res[0];
flint_stack_release();
}
NORM(res);
}
void fmpz_div_2exp(fmpz_t output, fmpz_t x, unsigned long exp)
{
unsigned long limbs = (exp >> FLINT_LG_BITS_PER_LIMB);
unsigned long bits = (exp & (FLINT_BITS - 1));
if ((x[0] == 0) || (limbs >= FLINT_ABS(x[0])))
{
output[0] = 0L;
return;
}
if (bits)
{
fmpz_t temp = fmpz_init(FLINT_ABS(x[0]) - limbs);
mpn_rshift(temp + 1, x + limbs + 1, FLINT_ABS(x[0]) - limbs, bits);
if ((long) x[0] >= 0L) temp[0] = x[0] - limbs;
else temp[0] = limbs + x[0];
NORM(temp);
fmpz_set(output, temp);
fmpz_clear(temp);
} else
{
F_mpn_copy(output + 1, x + limbs + 1, FLINT_ABS(x[0]) - limbs);
if ((long) x[0] >= 0L) output[0] = x[0] - limbs;
else output[0] = limbs + x[0];
}
}
void ConnectionManager::removeDuplicatePoints(Connection *c) {
bool removedSome = false;
bool remove = true;
// If we have three points in single line, we can remove the
// middle one
for (int point1 = 1; point1 < c->points.size(); ++point1) {
int point0 = point1 - 1;
int point2 = point1 + 1;
if (point2 == c->points.size()) {
break;
}
// if (p1.x == p2.x == p3.x) or (p1.y == p2.y == p3.y)
if ((NORM(c->points[point0].x()) == NORM(c->points[point1].x()) &&
NORM(c->points[point2].x()) == NORM(c->points[point1].x())) ||
(NORM(c->points[point0].y()) == NORM(c->points[point1].y()) &&
NORM(c->points[point2].y()) == NORM(c->points[point1].y()))) {
removedSome = true;
qDebug() << "removing duplicate" << point1;
removePoint(c, point1);
point1--;
}
}
// If we have two points on the same field in grid, we can remove
// the second one.
while(remove) {
remove = false;
for (int point1 = 0; point1 < c->points.size(); ++point1) {
QRect r1(c->points[point1].x() - 6, c->points[point1].y() - 6, 12, 12);
int point2 = point1 + 1;
for (; point2 < c->points.size(); ++point2) {
QRect r2(c->points[point2].x() - 6, c->points[point2].y() - 6, 12, 12);
if (r1.intersects(r2)) {
// remove = true;
break;
}
}
if (remove) {
for (int i = point1; i < point2; i++) {
removePoint(c, point1);
removedSome = true;
}
break;
}
}
}
if (removedSome) {
m_screen->repaint();
}
}
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);
}
void fmpz_tdiv_ui(fmpz_t output, const fmpz_t input, const unsigned long x)
{
output[0] = input[0];
unsigned long size = FLINT_ABS(input[0]);
mpn_divmod_1(output+1, input+1, size, x);
NORM(output);
}
static void compute_arrays(RGB pal[], int n)
{
int i;
float r, g, b, max, min, delta;
float l, h, mm, hsv, hls;
for (i=0; i<n; i++) {
idx[i] = i;
r = NORM(pal[i].red);
g = NORM(pal[i].green);
b = NORM(pal[i].blue);
max = MAX(r,g);
max = MAX(max,b);
min = MIN(r,g);
min = MIN(min,b);
mm = max+min;
l = mm * 0.5;
delta = max-min;
hsv = (max!=0.0 ? delta/max : 0.0);
if (delta==0.0) {
hls = 0.0;
h = -1.0;
} else {
hls = delta / ( (mm <= 1.0) ? mm: (2.0 - mm) );
h = r==max ? (g - b)/delta :
g==max ? (b - r)/delta + 2 :
(r - g)/delta + 4;
h *= 60;
if (h < 0) h += 360;
}
/* compute the YIQ luminance [0..1] */
y_yiq[i] = r*0.3 + g*0.59 + b*0.11;
l_hls[i] = l;
s_hls[i] = hls;
s_hsv[i] = hsv;
v_hsv[i] = max;
hue[i] = h;
} /* for ... */
}
void MainWindow::on_Metrics_clicked()
{QString code=ui->plainTextEdit->toPlainText();
ui->label_SrKMet->setText(QString::number(SrKolvoStrokMetodov(code)));
ui->label_SrKSCl->setText(QString::number(SrKolvoStrokClassov(code)));
MainWindow::MWC(code);MainWindow::NOC(code);
ui->label_RFC->setText(QString::number(RFC(code)));
ui->label_NORM->setText(QString::number(NORM(code)));
ui->label_Met->setText(QString::number(KolvoMetodov(code)));
ui->Kol_Class->setText(QString::number(KolvoClassov(code)));
ui->Include->setText(QString::number(KolvoStrokModuleyInc(code)));
}
void shNgin3dMove(float frontSpeed, float rightSpeed, float upSpeed, unsigned long dt)
{
// cashed version of conversion of latitude and longitude to points already exist in _Ngin3d_cameraTarget and _Ngin3d_cameraUp
float targetDir[3] = {_Ngin3d_cameraTarget[0]-_Ngin3d_cameraPosition[0], _Ngin3d_cameraTarget[1]-_Ngin3d_cameraPosition[1], _Ngin3d_cameraTarget[2]-_Ngin3d_cameraPosition[2]};
float rightDir[3] = {CROSS(targetDir, _Ngin3d_cameraUp)}; // targetDir and _Ngin3d_cameraUp should already be normalized
float upDir[3] = {0, 1, 0};
if (g_Ngin3d_uprightMode)
{
if (_Ngin3d_cameraUp[1] < 0)
upDir[1] = -1;
}
else
{
upDir[0] = _Ngin3d_cameraUp[0];
upDir[1] = _Ngin3d_cameraUp[1];
upDir[2] = _Ngin3d_cameraUp[2];
}
float speed[3] = {frontSpeed*targetDir[0]+upSpeed*upDir[0]+rightSpeed*rightDir[0],
frontSpeed*targetDir[1]+upSpeed*upDir[1]+rightSpeed*rightDir[1],
frontSpeed*targetDir[2]+upSpeed*upDir[2]+rightSpeed*rightDir[2]};
float gravity[3] = {g_Ngin3d_settings[NGIN3D_GRAVITY_X], g_Ngin3d_settings[NGIN3D_GRAVITY_Y], g_Ngin3d_settings[NGIN3D_GRAVITY_Z]};
float normGravity = NORM(gravity);
if (g_Ngin3d_uprightMode && !STATE_IN_AIR && normGravity > EPSILON)
{
float speedInDirectionOfGravity = DOT(speed, gravity)/normGravity/normGravity;
speed[0] -= gravity[0]*speedInDirectionOfGravity;
speed[1] -= gravity[1]*speedInDirectionOfGravity;
speed[2] -= gravity[2]*speedInDirectionOfGravity;
}
shNgin3dCollisionResult cr = g_Ngin3d_player.shNgin3dPOUpdate(dt, speed);
if (cr == NGIN3D_COLLISION_WITH_GROUND)
{
STATE_IN_AIR = false;
}
float cameraDisplacement[3] = {0, 0, 0};
if (g_Ngin3d_uprightMode)
{
if (STATE_CROUCHED)
cameraDisplacement[1] = g_Ngin3d_settings[NGIN3D_CROUCHED_HEIGHT];
else
cameraDisplacement[1] = g_Ngin3d_settings[NGIN3D_HEIGHT];
}
_Ngin3d_cameraPosition[0] = g_Ngin3d_player.position[0]+cameraDisplacement[0];
_Ngin3d_cameraPosition[1] = g_Ngin3d_player.position[1]+cameraDisplacement[1];
_Ngin3d_cameraPosition[2] = g_Ngin3d_player.position[2]+cameraDisplacement[2];
_Ngin3d_cameraTarget[0] = _Ngin3d_cameraPosition[0]+targetDir[0];
_Ngin3d_cameraTarget[1] = _Ngin3d_cameraPosition[1]+targetDir[1];
_Ngin3d_cameraTarget[2] = _Ngin3d_cameraPosition[2]+targetDir[2];
(*_Ngin3d_cameraPositionFunction)(_Ngin3d_cameraPosition);
(*_Ngin3d_cameraTargetFunction)(_Ngin3d_cameraTarget);
}
void
bp_camera_get_ray (camera_t *camera, int height, int width, ray_t *ray)
{
vector_t pos;
init_ray_computer (camera);
SCALE_ADD3 (pos, height, down_step, left_top);
SCALE_ADD3 (pos, width, right_step, pos);
ASSIGN (ray->orig, camera->location);
SUB (ray->dir, pos, camera->location);
NORM (ray->dir, ray->dir);
}
void fmpz_fdiv(fmpz_t res, const fmpz_t a, const fmpz_t b)
{
long a0 = a[0];
long b0 = b[0];
unsigned long sizea = FLINT_ABS(a0);
unsigned long sizeb = FLINT_ABS(b0);
while ((!a[sizea]) && (sizea)) sizea--;
while ((!b[sizeb]) && (sizeb)) sizeb--;
mp_limb_t mslimb;
fmpz_t temp;
if (sizeb == 0)
{
printf("Error: division by zero!\n");
abort();
} else if (sizea < sizeb) // Todo: make this deal with sizea == sizeb but a < b
{
if (((long) (a0 ^ b0) < 0L) && (a0))
{
res[0] = -1L;
res[1] = 1;
} else res[0] = 0;
return;
} else
{
temp = (fmpz_t) flint_stack_alloc(sizeb);
mpn_tdiv_qr(res+1, temp, 0, a+1, sizea, b+1, sizeb);
res[0] = sizea - sizeb + 1;
if ((long) (a0 ^ b0) < 0L) res[0] = -res[0];
NORM(res);
if ((long) (a0 ^ b0) < 0L)
{
unsigned long i = 0;
for (; i < sizeb; i++)
{
if (temp[i]) break;
}
if (i < sizeb)
{
fmpz_sub_ui_inplace(res, 1UL);
}
}
flint_stack_release();
}
}
void KntDepthHandler(freenect_device * dev, void * depth, uint32_t timestamp) {
ctxList_t * ptr = contexts;
/* Double buffer */
while (ptr) {
memcpy(ptr->ctx.dfb, ptr->ctx.dbb, 640*480*2);
/* Normalize */
for (int i = 0; i < 640*480; ++i) {
/* Cut values greater than 1024 as they are noisy */
if (ptr->ctx.dfb[i] > 1024)
ptr->ctx.depthBuf[i] = 0.0f;
else
ptr->ctx.depthBuf[i] = NORM(ptr->ctx.dfb[i]);
}
ptr = ptr->next;
}
}
void fmpz_sub_ui(fmpz_t output, const fmpz_t input, const unsigned long x)
{
unsigned long carry;
if (x)
{
if (!input[0])
{
output[1] = x;
output[0] = -1L;
} else if ((long) input[0] < 0)
{
carry = mpn_add_1(output + 1, input + 1, ABS(input[0]), x);
output[0] = input[0];
if (carry)
{
output[ABS(output[0])+1] = carry;
output[0]--;
}
} else if ((long) input[0] > 1L)
{
mpn_sub_1(output + 1, input + 1, input[0], x);
output[0] = input[0];
NORM(output);
} else
{
if (x <= input[1])
{
output[1] = input[1] - x;
if (!output[1]) output[0] = 0;
else output[0] = 1L;
} else
{
output[1] = x - input[1];
output[0] = -1L;
}
}
} else
{
fmpz_set(output, input);
}
}
//----------------------------------------------------------------------------------
void TrKMclass::SetParams(int _n_threads, uint32 _max_mem_size_in_gb)
{
kmer_len = 0;
prefix_len = 4;
n_bins = (1 << (2 * prefix_len));
bin_part_size = 1 << 18; //??
fastq_buffer_size = 1 << 20; //?
if (_max_mem_size_in_gb < 40)
kmers_buffer_size = 1 << 29; //??
else
kmers_buffer_size = 1 << 31; //??
AdjustToHardware(_n_threads);
max_mem_size = NORM(((uint64) _max_mem_size_in_gb) << 30, 2ull << 30, 1024ull << 30);
initialized = true;
}
void shNgin3dRoll(float deg)
{
if (!g_Ngin3d_uprightMode)
{
float targetDir[3] = {_Ngin3d_cameraTarget[0]-_Ngin3d_cameraPosition[0], _Ngin3d_cameraTarget[1]-_Ngin3d_cameraPosition[1], _Ngin3d_cameraTarget[2]-_Ngin3d_cameraPosition[2]};
float sdeg = sin(deg*PI/180);
float cdeg = cos(deg*PI/180);
float rotMat[3][3] = {{cdeg+targetDir[0]*targetDir[0]*(1-cdeg), targetDir[0]*targetDir[1]*(1-cdeg)-targetDir[2]*sdeg, targetDir[0]*targetDir[2]*(1-cdeg)+targetDir[1]*sdeg},
{targetDir[1]*targetDir[0]*(1-cdeg)+targetDir[2]*sdeg, cdeg+targetDir[1]*targetDir[1]*(1-cdeg), targetDir[1]*targetDir[2]*(1-cdeg)-targetDir[0]*sdeg},
{targetDir[2]*targetDir[0]*(1-cdeg)-targetDir[1]*sdeg, targetDir[2]*targetDir[1]*(1-cdeg)+targetDir[0]*sdeg, cdeg+targetDir[2]*targetDir[2]*(1-cdeg)}};
float newUp[3];
newUp[1] = DOT(rotMat[1], _Ngin3d_cameraUp);
if (_Ngin3d_upsideDownPossible || newUp[1] > 0)
{
newUp[0] = DOT(rotMat[0], _Ngin3d_cameraUp);
newUp[2] = DOT(rotMat[2], _Ngin3d_cameraUp);
float norm = NORM(newUp);
_Ngin3d_cameraUp[0] = newUp[0]/norm;
_Ngin3d_cameraUp[1] = newUp[1]/norm;
_Ngin3d_cameraUp[2] = newUp[2]/norm;
}
}
(*_Ngin3d_cameraUpFunction)(_Ngin3d_cameraUp);
}
void fmpz_sub_ui_inplace(fmpz_t output, const unsigned long x)
{
unsigned long carry;
if (x)
{
if (!output[0])
{
output[1] = x;
output[0] = -1L;
} else if ((long) output[0] < 0)
{
carry = mpn_add_1(output + 1, output + 1, ABS(output[0]), x);
if (carry)
{
output[ABS(output[0])+1] = carry;
output[0]--;
}
} else if ((long) output[0] > 1L)
{
mpn_sub_1(output + 1, output + 1, output[0], x);
NORM(output);
} else
{
if (x <= output[1])
{
output[1] -= x;
if (!output[1]) output[0] = 0;
} else
{
output[1] = x - output[1];
output[0] = -1L;
}
}
}
}
static int usb_pad_poll(PADState *ps, uint8_t *buf, int buflen)
{
LinuxPADState *s = (LinuxPADState*)ps;
uint8_t idx = 1 - s->padState.port;
ssize_t len;
if(idx > 1) return 0; //invalid port
int range = range_max(idx);
if(sendCrap)
{
// Setting to unpressed
memset(&wheel_data, 0, sizeof(wheel_data_t));
wheel_data.axis_x = range >> 1;
wheel_data.axis_y = 0xFF;
wheel_data.axis_z = 0xFF;
wheel_data.axis_rz = 0xFF;
wheel_data.hatswitch = 0x8;
pad_copy_data(idx, buf, wheel_data);
sendCrap = false;
return len;
}
// Events are raised if state changes, so keep old generic_data intact
//memset(&wheel_data, 0, sizeof(wheel_data_t));
struct js_event event;
int wtype = conf.WheelType[idx];
//Non-blocking read sets len to -1 and errno to EAGAIN if no new data
//TODO what happens when emulator is paused?
while((len = read(s->fd, &event, sizeof(event))) > -1)
{
//Dbg("Read js len: %d %d\n", len, errno);
if (len == sizeof(event))
{ // ok
if (event.type & JS_EVENT_AXIS)
{
//Dbg("Axis: %d, %d\n", event.number, event.value);
switch(s->axismap[event.number])
{
case ABS_X: wheel_data.axis_x = NORM(event.value, range); break;
case ABS_Y: wheel_data.axis_y = NORM(event.value, 0xFF); break;
case ABS_Z: wheel_data.axis_z = NORM(event.value, 0xFF); break;
//case ABS_RX: wheel_data.axis_rx = NORM(event.value, 0xFF); break;
//case ABS_RY: wheel_data.axis_ry = NORM(event.value, 0xFF); break;
case ABS_RZ: wheel_data.axis_rz = NORM(event.value, 0xFF); break;
//FIXME hatswitch mapping
//TODO Ignoring diagonal directions
case ABS_HAT0X:
case ABS_HAT1X:
if(event.value < 0 ) //left usually
wheel_data.hatswitch = PAD_HAT_W;
else if(event.value > 0 ) //right
wheel_data.hatswitch = PAD_HAT_E;
else
wheel_data.hatswitch = PAD_HAT_COUNT;
break;
case ABS_HAT0Y:
case ABS_HAT1Y:
if(event.value < 0 ) //up usually
wheel_data.hatswitch = PAD_HAT_N;
else if(event.value > 0 ) //down
wheel_data.hatswitch = PAD_HAT_S;
else
wheel_data.hatswitch = PAD_HAT_COUNT;
break;
default: break;
}
}
else if (event.type & JS_EVENT_BUTTON)
{
//Dbg("Button: %d, %d\n", event.number, event.value);
//TODO can have 12 bits for buttons?
if(event.number < 10)
{
//FIXME bit juggling
if(event.value)
wheel_data.buttons |= 1 << event.number; //on
else
wheel_data.buttons &= ~(1 << event.number); //off
}
}
}
else
{
Dbg("usb_pad_poll: unknown read error\n");
break;
}
}
//Dbg("call pad_copy_data\n");
pad_copy_data(idx, buf, wheel_data);
return buflen;
}
void correlate(int corr_func, dataset *d, field *c, FILE *out) {
ULONG *dd;
ULONG *dr;
ULONG *rr;
ULONG i, j;
int k;
p3 sep;
LDOUBLE l;
correlation_function cfunc;
/* Get the right function. */
switch (corr_func) {
case PEEBLES_HAUSER:
cfunc = &ph_corr;
break;
case DAVIS_PEEBLES:
cfunc = &dp_corr;
break;
case HAMILTON:
cfunc = &ham_corr;
break;
case LANDY_SZALAY:
default:
cfunc = &ls_corr;
break;
}
/* Allocate. */
dd = (ULONG*)malloc(d->num_bins * sizeof(ULONG));
check_malloc(dd, __LINE__);
dr = (ULONG*)malloc(d->num_bins * sizeof(ULONG));
check_malloc(dr, __LINE__);
rr = (ULONG*)malloc(d->num_bins * sizeof(ULONG));
check_malloc(rr, __LINE__);
/* Zero the storages. */
memset(dd, 0, d->num_bins * sizeof(ULONG));
memset(dr, 0, d->num_bins * sizeof(ULONG));
memset(rr, 0, d->num_bins * sizeof(ULONG));
/* Calculate DD and DR pairs, and add for each bin. */
/* XXX: We require that one pair can only end up in one bin, so we can
* stop going through the bins as soon as we find a match. This means
* that overlapping bins to achieve some effect will not result in
* expected behaviour. */
printf("DD and DR pairs\n");
for (i=0; i < d->data.num_pts; i++) {
/* Print some progress info. */
printf("%lu ", i);
if (i != 0 && (i % 10 == 0))
printf("\n");
for (j=i+1; j < d->data.num_pts; j++) {
sep.x = d->data.pts[i].x - d->data.pts[j].x;
sep.y = d->data.pts[i].y - d->data.pts[j].y;
sep.z = d->data.pts[i].z - d->data.pts[j].z;
l = NORM(sep);
for (k=0; k < d->num_bins; k++) {
if (d->bins[k][0] <= l
&& l <= d->bins[k][1]) {
dd[k]+=2;
break;
}
}
}
for (j=0; j < c->num_pts; j++) {
sep.x = d->data.pts[i].x - c->pts[j].x;
sep.y = d->data.pts[i].y - c->pts[j].y;
sep.z = d->data.pts[i].z - c->pts[j].z;
l = NORM(sep);
for (k=0; k < d->num_bins; k++) {
if (d->bins[k][0] <= l
&& l <= d->bins[k][1]) {
dr[k]++;
break;
}
}
}
}
printf("\n");
/* Calculate RR pairs. */
printf("RR pairs\n");
for (i=0; i < c->num_pts; i++) {
/* Print some progress info. */
printf("%lu ", i);
if (i != 0 && (i % 10 == 0))
printf("\n");
for (j=i+1; j < c->num_pts; j++) {
sep.x = c->pts[i].x - c->pts[j].x;
sep.y = c->pts[i].y - c->pts[j].y;
sep.z = c->pts[i].z - c->pts[j].z;
l = NORM(sep);
for (k=0; k < d->num_bins; k++) {
if (d->bins[k][0] <= l
&& l <= d->bins[k][1]) {
rr[k]+=2;
break;
}
}
}
}
printf("\n");
/* Calculate the correlation and Poisson error
* for each bin. Then print bin low bound, high bound, ksi and error
* on each row. */
{
LDOUBLE ksi, error;
LDOUBLE rat = c->num_pts/(LDOUBLE)d->data.num_pts;
for (k=0; k < d->num_bins; k++) {
printf("Bin %d [%lg, %lg] ",
k, d->bins[k][0], d->bins[k][1]);
printf("dd %lu ", dd[k]);
printf("dr %lu ", dr[k]);
printf("rr %lu\n", rr[k]);
if (rr[k] == 0) {
/* XXX: Something like this is what _should_
* be done, however, it would require
* enabling GNU extensions or somesuch. */
/*
ksi = error = NAN;
*/
fprintf(out, "%lg %lg %s %s\n",
d->bins[k][0], d->bins[k][1],
"NaN", "NaN");
}
else {
/* Use the given estimator. */
ksi = cfunc(rat, dd[k], dr[k], rr[k]);
/* For the Poisson errors, we have:
* (1+ksi)/sqrt(x), where x can be DD,
* (N/N_r)*DR or (N/N_r)^2*RR. Of these,
* Martinez & Saar suggest using one of the
* latter two. Of these, again the latter
* is more convenient, as the RR counts should
* be consistently large. */
/*
error = (1 + ksi)/sqrt(dd[k]);
error = (1 + ksi)/sqrt(dr[k]/rat);
*/
error = (1 + ksi)/sqrt(rr[k]/(rat*rat));
fprintf(out, "%lg %lg %lg %lg\n",
d->bins[k][0], d->bins[k][1],
ksi, error);
}
}
}
/* Cleanup. */
free(dd);
free(dr);
free(rr);
}
