void SweepAndPrune::AddObject(const Eigen::AlignedBox3f& box, void* obj) {
auto maxvec = box.max();
auto minvec = box.min();
BBox toAdd = { 0 };
toAdd.userRef = obj;
_objects.push_back(toAdd);
unsigned int boxIdx = static_cast<unsigned int>(_objects.size() - 1);
_objectMap[obj] = boxIdx;
for (unsigned int i = 0; i < 3; ++i) {
EndPoint min;
EndPoint max;
//make sure we are not screwing around with the high bit
assert(boxIdx <= (std::numeric_limits<unsigned int>::max() & ~EndPoint::max));
min.box = boxIdx;
max.box = boxIdx | EndPoint::max;
min.value = std::numeric_limits<float>::max();
max.value = std::numeric_limits<float>::max();
//this stuff is slow as balls
_axis[i]->insert(_axis[i]->end() - 1, min);
_objects[boxIdx].min[i] = static_cast<unsigned int>(_axis[i]->size()) - 2;
updateAxis(i, static_cast<int>(_axis[i]->size()) - 2, minvec(i));
_axis[i]->insert(_axis[i]->end() - 1, max);
_objects[boxIdx].max[i] = static_cast<unsigned int>(_axis[i]->size()) - 2;
updateAxis(i, static_cast<int>(_axis[i]->size()) - 2, maxvec(i));
}
}
void SweepAndPrune::UpdateObject(const Eigen::AlignedBox3f& aabb, void* obj) {
auto min = aabb.min();
auto max = aabb.max();
auto& box = _objects[_objectMap.at(obj)];
for (unsigned int i = 0; i < 3; ++i) {
updateAxis(i, box.min[i], min(i));
updateAxis(i, box.max[i], max(i));
}
}
//----------------------------------------
void ofNode::createMatrix() {
localTransformMatrix = glm::translate(glm::mat4(1.0), toGlm(position));
localTransformMatrix = localTransformMatrix * glm::toMat4((const glm::quat&)orientation);
localTransformMatrix = glm::scale(localTransformMatrix, toGlm(scale));
updateAxis();
}
void RigidAlignment::saveSphere(const char *dir)
{
for (int i = 0; i < m_nSubj; i++)
{
const float *axis = &faxis[i * 3];
float axis2[3];
updateAxis(m_rot[i * 3 + 1], m_rot[i * 3 + 2], axis, axis2);
Vector ax(axis), ax2(axis2);
float inner = ax * ax2;
if (inner > 1) inner = 1;
else if (inner < -1) inner = -1;
float deg = acos(inner);
Mesh *sphere = new Mesh();
sphere->openFile(m_spherename);
// matrix
Vector ax3 = ax.cross(ax2); ax3.unit();
if (ax3.norm() != 0)
sphere->rotation(ax3.fv(), deg);
sphere->rotation(axis2, m_rot[i * 3]);
// output
char filename[1024];
sprintf(filename, "%s/%s.vtk", dir, m_filename[i]);
sphere->saveFile(filename, "vtk");
delete sphere;
}
}
// Update the scale after there is a zoom in or out
void SimpleChart::updateScale()
{
if (scale <= 9) pixelsByGroup = 1;
else if (scale == 10) pixelsByGroup = 10;
pixelsByUnit = AXIS_SIZE / ((int)scale * 2);
updateAxis();
}
//----------------------------------------
void ofNode::createMatrix() {
//if(isMatrixDirty) {
// isMatrixDirty = false;
localTransformMatrix.makeScaleMatrix(scale);
localTransformMatrix.rotate(orientation);
localTransformMatrix.setTranslation(position);
updateAxis();
}
//----------------------------------------
void ofNode::setTransformMatrix(const ofMatrix4x4 &m44) {
localTransformMatrix = m44;
ofQuaternion so;
localTransformMatrix.decompose(position, orientation, scale, so);
updateAxis();
onPositionChanged();
onOrientationChanged();
onScaleChanged();
}
void ComparativeBarChart::update()
{
updateSizes();
osg::Vec3 scale(_width,1.0,_height);
osg::Matrix scaleMat;
scaleMat.makeScale(scale);
_bgScaleMT->setMatrix(scaleMat);
updateAxis();
updateGraph();
}
void UpdateAxisActionJob::run()
{
// Note: we assume axis/action are not really shared:
// there's no benefit in sharing those when it comes to computing
LogicalDevice *device = m_handler->logicalDeviceManager()->data(m_handle);
if (!device->isEnabled())
return;
updateAction(device);
updateAxis(device);
}
//----------------------------------------
void ofNode::setTransformMatrix(const ofMatrix4x4 &m44) {
localTransformMatrix = m44;
ofVec3f position;
ofQuaternion orientation;
ofVec3f scale;
ofQuaternion so;
localTransformMatrix.decompose(position, orientation, scale, so);
this->position = position;
this->orientation = orientation;
this->scale = scale;
updateAxis();
onPositionChanged();
onOrientationChanged();
onScaleChanged();
}
//----------------------------------------
void ofNode::setTransformMatrix(const glm::mat4 &m44) {
localTransformMatrix = m44;
glm::vec3 scale;
glm::quat orientation;
glm::vec3 translation;
glm::vec3 skew;
glm::vec4 perspective;
glm::decompose(m44, scale, orientation, translation, skew, perspective);
this->position = translation;
this->scale = scale;
this->orientation = orientation;
updateAxis();
onPositionChanged();
onOrientationChanged();
onScaleChanged();
}
void DiagnosticsDisplay::onInitialize()
{
static int counter = 0;
scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
orbit_node_ = scene_node_->createChildSceneNode(); // ??
line_ = new rviz::BillboardLine(context_->getSceneManager(), scene_node_);
msg_ = new rviz::MovableText("not initialized", "Liberation Sans", 0.05);
msg_->setTextAlignment(rviz::MovableText::H_CENTER,
rviz::MovableText::V_ABOVE);
frame_id_property_->setFrameManager(context_->getFrameManager());
orbit_node_->attachObject(msg_);
msg_->setVisible(false);
orbit_theta_ = M_PI * 2.0 / 6 * counter++;
updateLineWidth();
updateAxis();
updateDiagnosticsNamespace();
updateRadius();
updateRosTopic();
updateFontSize();
}
DiagnosticsDisplay::DiagnosticsDisplay()
: rviz::Display(), msg_(0)
{
ros_topic_property_
= new rviz::RosTopicProperty(
"Topic", "/diagnostics_agg",
ros::message_traits::datatype<diagnostic_msgs::DiagnosticArray>(),
"diagnostic_msgs::DiagnosticArray topic to subscribe to.",
this, SLOT( updateRosTopic() ));
frame_id_property_ = new rviz::TfFrameProperty(
"frame_id", rviz::TfFrameProperty::FIXED_FRAME_STRING,
"the parent frame_id to visualize diagnostics",
this, 0, true);
diagnostics_namespace_property_ = new rviz::EditableEnumProperty(
"diagnostics namespace", "/",
"diagnostics namespace to visualize diagnostics",
this, SLOT(updateDiagnosticsNamespace()));
radius_property_ = new rviz::FloatProperty(
"radius", 1.0,
"radius of diagnostics circle",
this, SLOT(updateRadius()));
line_width_property_ = new rviz::FloatProperty(
"line width", 0.03,
"line width",
this, SLOT(updateLineWidth()));
axis_property_ = new rviz::EnumProperty(
"axis", "x",
"axis",
this, SLOT(updateAxis()));
axis_property_->addOption("x", 0);
axis_property_->addOption("y", 1);
axis_property_->addOption("z", 2);
font_size_property_ = new rviz::FloatProperty(
"font size", 0.05,
"font size",
this, SLOT(updateFontSize()));
}
// Update the filter based on most recent values
// dT is in milliseconds
// g* is gyro rotation rate in degrees/s
// a* is current angle in degrees
// heading is magnetometer heading in degrees
void IMU::update(int dT, float gx, float gy, float gz, float ax, float ay, float az, float heading){
updateAxis(ROLL, dT, gx, ax);
updateAxis(PITCH, dT, gy, ay);
updateAxis(YAW, dT, gz, heading);
}
void RigidAlignment::update(void)
{
// new point
for (int i = 0; i < m_nSubj; i++)
{
const float *axis = &faxis[i * 3];
// new axis
float axis2[3];
updateAxis(m_rot[i * 3 + 1], m_rot[i * 3 + 2], axis, axis2);
Vector ax(axis), ax2(axis2);
float inner = ax * ax2;
if (inner > 1) inner = 1;
else if (inner < -1) inner = -1;
float deg = acos(inner);
// matrix
float mat[9];
Vector ax3 = ax.cross(ax2); ax3.unit();
if (ax3.norm() != 0)
{
Coordinate::rotation(ax3.fv(), deg, mat);
// axis rotation
for (int j = 0; j < m_nLM; j++)
{
float newp[3];
float *p = &fpoint[(i * m_nLM + j) * 3];
int id = m_point[i][j];
memcpy(p, m_sphere->vertex(id)->fv(), sizeof(float) * 3);
Coordinate::rotPoint(p, mat, newp);
memcpy(p, newp, sizeof(float) * 3);
}
}
// matrix
Coordinate::rotation(axis2, m_rot[i * 3], mat);
// rotation
for (int j = 0; j < m_nLM; j++)
{
float newp[3];
float *p = &fpoint[(i * m_nLM + j) * 3];
Coordinate::rotPoint(p, mat, newp);
memcpy(p, newp, sizeof(float) * 3);
}
}
// mean
memset(fmean, 0, sizeof(float) * m_nLM * 3);
for (int i = 0; i < m_nLM; i++)
for (int j = 0; j < m_nSubj; j++)
for (int k = 0; k < 3; k++)
fmean[i * 3 + k] += fpoint[(j * m_nLM + i) * 3 + k] / m_nSubj;
for (int i = 0; i < m_nLM; i++)
{
float norm = fmean[i * 3] * fmean[i * 3] + fmean[i * 3 + 1] * fmean[i * 3 + 1] + fmean[i * 3 + 2] * fmean[i * 3 + 2];
norm = sqrt(norm);
for (int j = 0; j < 3; j++)
fmean[i * 3 + j] /= norm;
}
/*for (int i = 0; i < m_nLM; i++)
cout << fmean[i * 3] << " " << fmean[i * 3 + 1] << " " << fmean[i * 3 + 2] << endl;*/
}
