/**
* Find out if the current number of children at a given position should be changed
* (player has moved relative to it)
*/
bool SphereFraction::autoNumUpdateReq(SimpleVec3d campos_relative, float intensity,
SimpleVec3d position)
{
float camdist = getVectorLength(campos_relative - position);
if (camdist == 0) return false; /* prevent infinite children */
return camdist < m_fracsize * intensity;
}
SimpleVec3d SimpleVec3d::normalize()
{
double len = getVectorLength(*this);
x /= len;
y /= len;
z /= len;
return *this;
}
void LaserCamHyperopt::colorizeCloudPinhole(PointCloudIntensity::Ptr input_cloud,
PointCloudRGB::Ptr output_cloud_rgb,
PointCloudIntensity::Ptr output_cloud_visible_for_cam)
{
output_cloud_rgb->clear();
output_cloud_visible_for_cam->clear();
// generate a mapping to save the indices of points that correspond to a specific pixel
std::vector<std::vector<std::vector<int> > > mapping_points_to_pixels (m_current_image.rows, std::vector<std::vector<int> >(m_current_image.cols, std::vector<int>(0) ) );
std::vector<std::vector<std::vector<float> > > mapping_distance_of_points_to_pixels (m_current_image.rows, std::vector<std::vector<float> >(m_current_image.cols, std::vector<float>(0) ) );
int index_counter = 0;
for(auto& point : input_cloud->points)
{
// check if point is not behind the camera
if(point.z < 0.0)
continue;
cv::Point3d pos(point.x, point.y, point.z);
cv::Point2d rectPixel = m_cam_model.project3dToPixel(pos);
cv::Point2d pixel = m_cam_model.unrectifyPoint(rectPixel);
pixel = double(m_current_image.cols / m_cam_model.fullResolution().width) * pixel;
cv::Point2i ipix(pixel.x, pixel.y);
// check if pixel index is valid
if(ipix.x < 0 || ipix.y < 0 || ipix.x >= (int)m_current_image.cols || ipix.y >= (int)m_current_image.rows)
continue;
cv::Vec3b color = m_current_image.at<cv::Vec3b>(ipix.y, ipix.x);
pcl::PointXYZRGB colorized_point;
colorized_point.x = point.x;
colorized_point.y = point.y;
colorized_point.z = point.z;
colorized_point.r = color[2];
colorized_point.g = color[1];
colorized_point.b = color[0];
output_cloud_rgb->push_back(colorized_point);
output_cloud_visible_for_cam->push_back(point);
mapping_points_to_pixels[ipix.y][ipix.x].push_back(index_counter);
mapping_distance_of_points_to_pixels[ipix.y][ipix.x].push_back(getVectorLength(colorized_point));
index_counter++;
}
ROS_DEBUG_STREAM("filtering occluded points in cloud");
filterOccludedPoints(output_cloud_rgb, output_cloud_visible_for_cam, mapping_points_to_pixels, mapping_distance_of_points_to_pixels);
}
void randomPlanetPosition(SimpleVec3d *pos, SimpleVec3d *vel, SimpleVec3d gravcen, SimpleVec3d axis)
{
// Generate random rotation by the given parameters
double randangle = 1. * rand() / RAND_MAX * 2 * PI;
glm::quat randquat = glm::angleAxis((float)randangle, axis.normalize().toVec3());
// Rotate velocity of the Planet
double vel_len = getVectorLength(*vel);
*vel = rotateVecByQuat((*vel).normalize(), randquat).normalize();
(*vel) *= vel_len;
// Rotate the relative position of the planet around the gravity center
// (sun / star / other planet in case this "Planet" is actually a moon)
SimpleVec3d pos_rel = *pos - gravcen;
double pos_len = getVectorLength(pos_rel);
pos_rel = rotateVecByQuat(pos_rel, randquat).normalize();
pos_rel *= pos_len;
*pos = gravcen + pos_rel;
}
vector DAX_load_data(int *array, int size) {
int lines;
struct timespec tstart, tend;
vector valuesVec;
clock_gettime(CLOCK_HIGHRES, &tstart);
valuesVec = vector_load_from_array(array, size, INTEGER, sizeof(int));
clock_gettime(CLOCK_HIGHRES, &tend);
fprintf(stderr, "%.6f seconds to load %d integers from array using DAX library\n",
(tend.tv_sec + 1.0e-9*tend.tv_nsec) -
(tstart.tv_sec + 1.0e-9*tstart.tv_nsec), getVectorLength(valuesVec));
return valuesVec;
}
/**
* \brief Creates a new SphereFraction
* \param p The bounds of the new SphereFraction
*
* Only to be used to created children SphereFractions.
*/
SphereFraction::SphereFraction(SphericalVector3f *p) :
m_vertices_id(-1),
m_children_static(false)
{
m_children.clear();
for (uint8_t i = 0; i < 4; i++)
m_positions[i] = p[i];
SphericalVector3f diffvector_spherical(m_positions[0].radius,
m_positions[1].inclination - m_positions[3].inclination + PI / 2,
m_positions[1].azimuth - m_positions[3].azimuth);
SimpleVec3d diffvector = SimpleVec3d(SphericalVector3f(m_positions[0].radius, PI / 2, 0))
- SimpleVec3d(diffvector_spherical);
m_fracsize = getVectorLength(diffvector);
}
void ofxGrabbableSource::update(){//TODO radius und zeit für bestimmung wie oft und wieviele!!!
if(!run)
return;
float alphaStep;
float percentPerFrame;
float radius;
if(modi == 0){
alphaStep = settings->particleAlphaStep0;
percentPerFrame = settings->percemtPerFrame0;
radius = settings->particleRadius0;
}else{
alphaStep = settings->particleAlphaStep1;
percentPerFrame = settings->percemtPerFrame1;
radius = settings->particleRadius1;
}
radius = radius>1 ? radius : 1;
float particleArea = radius * radius * PI;
int particlePerFrame = ceil(percentPerFrame / (1.f/(calcArea/particleArea)));
for(int i=0;i<particlePerFrame;++i){
ParticleQ3 * p;
do{
p = (ParticleQ3*)particleSystem->getNext();
}while(!p->bFree);
float alpha = ofRandom(0,360);
ofVec3f v(0,1);
v.rotate(alpha,ofVec3f(0,0,1));
if(modi==1){
v.scale(calcRadius);
}else{
v.scale(ofRandom(0,calcRadius));
}
v += *this;
p->x = v.x+tx;
p->y = v.y+ty;
p->setFree(false);
p->setMode(alphaStep,getVectorLength(),radius);
p->xv = p->yv = 0;
p->resetForce();
}
}
void LaserCamHyperopt::filterOccludedPoints(PointCloudRGB::Ptr output_cloud_rgb, PointCloudIntensity::Ptr output_cloud_visible_for_cam,
const std::vector<std::vector<std::vector<int> > > &mapping_points_to_pixels,
const std::vector<std::vector<std::vector<float> > > &mapping_distance_of_points_to_pixels)
{
// check each pixel of the image or neighborhood of pixels
// if it corresponds to more than one cloud point, keep only closer point
PointCloudRGB::Ptr filtered_rgb_cloud (new PointCloudRGB);
PointCloudIntensity::Ptr filtered_intens_cloud (new PointCloudIntensity);
std::vector<bool> indices_to_delete(output_cloud_rgb->size(), false);
int neighborhood_radius_in_pixels = 3;//m_pixel_neighborhood();
float epsilon_dist_to_ray = 0.01;//m_epsilon_dist_to_ray();
for(int row_index = 0; row_index < m_current_image.rows; row_index++)
{
for(int col_index = 0; col_index < m_current_image.cols; col_index++)
{
// if there is a point to check
if(mapping_points_to_pixels[row_index][col_index].size() > 0)
{
// get index of point with min distance to origin/cam
auto min_dist_element_index = std::distance(mapping_distance_of_points_to_pixels[row_index][col_index].begin(),
std::min_element(mapping_distance_of_points_to_pixels[row_index][col_index].begin(),
mapping_distance_of_points_to_pixels[row_index][col_index].end()));
// select one point for the ray which goes through the origin and this point
int index_of_point_for_ray = mapping_points_to_pixels[row_index][col_index][min_dist_element_index];
pcl::PointXYZ point_for_ray(output_cloud_rgb->points[index_of_point_for_ray].x,
output_cloud_rgb->points[index_of_point_for_ray].y,
output_cloud_rgb->points[index_of_point_for_ray].z);
float distance_origin_to_ray_point = mapping_distance_of_points_to_pixels[row_index][col_index][min_dist_element_index] + 0.03;
// compute distances of the neighboring points to the current ray and colorize those with a small distance
for(int local_row_index = std::max(0, row_index - neighborhood_radius_in_pixels);
local_row_index < std::min(m_current_image.rows,row_index + neighborhood_radius_in_pixels); local_row_index++)
{
for(int local_col_index = std::max(0, col_index - neighborhood_radius_in_pixels);
local_col_index < std::min(m_current_image.cols, col_index + neighborhood_radius_in_pixels); local_col_index++)
{
if(mapping_points_to_pixels[local_row_index][local_col_index].size() > 0)
{
for(unsigned int point_in_pixel = 0; point_in_pixel < mapping_points_to_pixels[local_row_index][local_col_index].size(); point_in_pixel++)
{
int index_of_current_point = mapping_points_to_pixels[local_row_index][local_col_index][point_in_pixel];
// the distance to the ray checks more or less the same as the size of the neighborhood does
// but geometrically more precise, since a pixel distance depends on the distance of the object to the camera
// if there are performance issues it could be discarded
float distance_to_ray = distPointToRayThroughOrigin(output_cloud_rgb->points[index_of_current_point], point_for_ray);
float distance_origin_to_point = getVectorLength(output_cloud_rgb->points[index_of_current_point]);
if((distance_to_ray < epsilon_dist_to_ray) && (distance_origin_to_point > distance_origin_to_ray_point))
{
indices_to_delete[index_of_current_point] = true;
}
}
}
}
}
}
}
}
for(unsigned int index = 0; index < indices_to_delete.size(); index++)
{
if(indices_to_delete[index] == false)
{
filtered_rgb_cloud->push_back(output_cloud_rgb->points[index]);
filtered_intens_cloud->push_back(output_cloud_visible_for_cam->points[index]);
}
}
filtered_rgb_cloud->swap(*output_cloud_rgb);
filtered_intens_cloud->swap(*output_cloud_visible_for_cam);
}
// distance between a point x_0 to a ray that goes through the origin x_1 and the point x_2
// http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
float distPointToRayThroughOrigin(const pcl::PointXYZRGB &x_0, const pcl::PointXYZ &x_2)
{
float numerator = getVectorLength(crossProduct(x_2, pcl::PointXYZ(-x_0.x, -x_0.y, -x_0.z)));
float denominator = getVectorLength(x_2);
return numerator/denominator;
}
void LaserCamHyperopt::colorizeCloudOmni(PointCloudIntensity::Ptr input_cloud,
PointCloudRGB::Ptr output_cloud_rgb,
PointCloudIntensity::Ptr output_cloud_visible_for_cam)
{
output_cloud_rgb->clear();
output_cloud_visible_for_cam->clear();
// generate a mapping to save the indices of points that correspond to a specific pixel
std::vector<std::vector<std::vector<int> > > mapping_points_to_pixels (m_current_image.rows, std::vector<std::vector<int> >(m_current_image.cols, std::vector<int>(0) ) );
std::vector<std::vector<std::vector<float> > > mapping_distance_of_points_to_pixels (m_current_image.rows, std::vector<std::vector<float> >(m_current_image.cols, std::vector<float>(0) ) );
// TODO: replace hardcoded values (for overhead camera of Momaro) by parameters
double xi = 1.9460225633816206;
// if xi < 1 then xi else 1/xi
double fov_parameter = ( xi < 1.0 ) ? xi : 1.0/xi;
double k1 = -0.018051944074190075;
double k2 = -0.05004787126701355;
double p1 = -0.0048353740034077644;
double p2 = 0.002677318917962319;
int index_counter = 0;
for(auto& point : input_cloud->points)
{
// check if point is not behind the camera
if(point.z < 0.0)
continue;
cv::Point3d pos(point.x, point.y, point.z);
double distance = sqrt(pos.x * pos.x + pos.y * pos.y + pos.z * pos.z);
// check if point is in the field of view
if(pos.z > -(fov_parameter * distance))
{
cv::Point2d outKeyPoint;
// euclidean to principal plane
double rz = 1.0 / (pos.z + xi * distance);
outKeyPoint.x = pos.x * rz;
outKeyPoint.y = pos.y * rz;
// distort wrt Radtan model
double mx2_u = outKeyPoint.x * outKeyPoint.x;
double my2_u = outKeyPoint.y * outKeyPoint.y;
double mxy_u = outKeyPoint.x * outKeyPoint.y;
double rho2_u = mx2_u + my2_u;
double rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;
outKeyPoint.x += outKeyPoint.x * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u);
outKeyPoint.y += outKeyPoint.y * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);
// principal plane -> image plane
outKeyPoint.x = m_cam_model.fx() * outKeyPoint.x + m_cam_model.cx();
outKeyPoint.y = m_cam_model.fy() * outKeyPoint.y + m_cam_model.cy();
cv::Point2i ipix(outKeyPoint.x, outKeyPoint.y);
// check if pixel index is valid
if(ipix.x < 0 || ipix.y < 0 || ipix.x >= (int)m_current_image.cols || ipix.y >= (int)m_current_image.rows)
continue;
cv::Vec3b color = m_current_image.at<cv::Vec3b>(ipix.y, ipix.x);
pcl::PointXYZRGB colorized_point;
colorized_point.x = point.x;
colorized_point.y = point.y;
colorized_point.z = point.z;
colorized_point.r = color[2];
colorized_point.g = color[1];
colorized_point.b = color[0];
output_cloud_rgb->push_back(colorized_point);
output_cloud_visible_for_cam->push_back(point);
mapping_points_to_pixels[ipix.y][ipix.x].push_back(index_counter);
mapping_distance_of_points_to_pixels[ipix.y][ipix.x].push_back(getVectorLength(colorized_point));
index_counter++;
}
}
filterOccludedPoints(output_cloud_rgb, output_cloud_visible_for_cam, mapping_points_to_pixels, mapping_distance_of_points_to_pixels);
}
void PhysicsInformation::render(int window_w, int window_h)
{
if (m_hidden) return;
glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
// Calculate size of 1 pixel
float ps_w = 2. / window_w; // pixelsize x-direction
float ps_h = 2. / window_h; // pixelsize y-direction
// Timelapse
std::ostringstream conversion_tl;
conversion_tl<<game->getGameSpeed();
std::string gamespeed_str = conversion_tl.str();
std::string timelapse_text = "Timelapse: " + gamespeed_str + "x";
glRasterPos2f(-1 + 5 * ps_w, 1 - 23 * ps_h);
glutBitmapString(GLUT_BITMAP_HELVETICA_18, (unsigned char *)timelapse_text.c_str());
// Camera type
std::string camtype;
switch (game->getSpaceship()->getCamBind())
{
case CAMERA_BOUND:
camtype = "bound";
break;
case CAMERA_RELATIVE_ROT:
camtype = "relative with rotation";
break;
case CAMERA_RELATIVE:
camtype = "relative";
break;
case CAMERA_FREE:
camtype = "free";
break;
}
std::string camtype_text = "Camera: " + camtype;
glRasterPos2f(-1 + 5 * ps_w, 1 - 46 * ps_h);
glutBitmapString(GLUT_BITMAP_HELVETICA_18, (unsigned char *)camtype_text.c_str());
// Ship Speed
float shipvel = getVectorLength(game->getSpaceship()->getVelocity()) / USC;
std::string speedtext = "Speed: " + std::to_string(shipvel) + " km/s";
glRasterPos2f(-1 + 5 * ps_w, 1 - 69 * ps_h);
glutBitmapString(GLUT_BITMAP_HELVETICA_18, (unsigned char *)speedtext.c_str());
if (shipvel > SPEED_OF_LIGHT)
{
glColor4f(1.0f, 0.0f, 0.0f, 1.0f);
glRasterPos2f(1 - 150 * ps_w, 1 - 23 * ps_h);
glutBitmapString(GLUT_BITMAP_HELVETICA_18, (unsigned char *)"SPEED OF LIGHT!");
glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
}
// Gravity Acceleration
SimpleVec3d gravacc_v = game->getSpaceship()->getGravityAcc();
float gravacc = getVectorLength(gravacc_v) * 1000 / USC; // / 1000 for km/s² to m/s²
std::string gravacc_text = "Acceleration: " + std::to_string(gravacc) + " m/s ";
gravacc_text.push_back(0xB2); // ² character
glRasterPos2f(-1 + 5 * ps_w, 1 - 92 * ps_h);
glutBitmapString(GLUT_BITMAP_HELVETICA_18, (unsigned char *)gravacc_text.c_str());
}
double angleBetween (SimpleVec3d a, SimpleVec3d b)
{
return acos(dotProduct(a, b) / (getVectorLength(a) * getVectorLength(b)));
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
/* matlab usage: pdos(data,cone,params); */
Data *d;
Cone *k;
const Sol *sol;
const mxArray *data;
const mxArray *A_mex;
const mxArray *b_mex;
const mxArray *c_mex;
const mxArray *f_mex;
const mxArray *l_mex;
const mxArray *q_mex;
const double *q_mex_vals;
const mxArray *x0;
const mxArray *y0;
const mxArray *s0;
const mxArray *cone;
const mxArray *params;
idxint i;
if (nrhs != 3){
mexErrMsgTxt("Three arguments are required in this order: data struct, cone struct, params struct");
}
if (nlhs > 4) {
mexErrMsgTxt("PDOS has up to 4 output arguments only.");
}
d = mxMalloc(sizeof(Data));
k = mxMalloc(sizeof(Cone));
d->p = mxMalloc(sizeof(Params));
data = prhs[0];
cone = prhs[1];
params = prhs[2];
A_mex = (mxArray *) mxGetField(data,0,"A");
if(A_mex == NULL) {
mxFree(d->p); mxFree(d); mxFree(k);
mexErrMsgTxt("Data struct must contain a `A` entry.");
}
if (!mxIsSparse(A_mex)){
mxFree(d->p); mxFree(d); mxFree(k);
mexErrMsgTxt("Input matrix A must be in sparse format (pass in sparse(A))");
}
if (mxIsComplex(A_mex)) {
mxFree(d->p); mxFree(d); mxFree(k);
mexErrMsgTxt("Input matrix A cannot be complex");
}
b_mex = (mxArray *) mxGetField(data,0,"b");
if(b_mex == NULL) {
mxFree(d->p); mxFree(d); mxFree(k);
mexErrMsgTxt("Data struct must contain a `b` entry.");
}
if(mxIsSparse(b_mex)) {
mxFree(d->p); mxFree(d); mxFree(k);
mexErrMsgTxt("Input vector b must be dense (pass in full(b))");
}
if (mxIsComplex(b_mex)) {
mxFree(d->p); mxFree(d); mxFree(k);
mexErrMsgTxt("Input vector b cannot be complex");
}
c_mex = (mxArray *) mxGetField(data,0,"c");
if(c_mex == NULL) {
mxFree(d->p); mxFree(d); mxFree(k);
mexErrMsgTxt("Data struct must contain a `c` entry.");
}
if(mxIsSparse(c_mex)) {
mxFree(d->p); mxFree(d); mxFree(k);
mexErrMsgTxt("Input vector c must be dense (pass in full(c))");
}
if (mxIsComplex(c_mex)) {
mxFree(d->p); mxFree(d); mxFree(k);
mexErrMsgTxt("Input vector c cannot be complex");
}
x0 = (mxArray *) mxGetField(data,0,"x0");
if(x0 != NULL && mxIsSparse(x0)) {
mxFree(d->p); mxFree(d); mxFree(k);
mexErrMsgTxt("Initial vector x0 must be dense (pass in full(x0))");
}
if (x0 != NULL && mxIsComplex(x0)) {
mxFree(d->p); mxFree(d); mxFree(k);
mexErrMsgTxt("Initial vector x0 cannot be complex");
}
y0 = (mxArray *) mxGetField(data,0,"y0");
if(y0 != NULL && mxIsSparse(y0)) {
mxFree(d->p); mxFree(d); mxFree(k);
mexErrMsgTxt("Initial vector y0 must be dense (pass in full(y0))");
}
if (y0 != NULL && mxIsComplex(y0)) {
mxFree(d); mxFree(k);
mexErrMsgTxt("Initial vector y0 cannot be complex");
}
s0 = (mxArray *) mxGetField(data,0,"s0");
if(s0 != NULL && mxIsSparse(s0)) {
mxFree(d); mxFree(k);
mexErrMsgTxt("Initial vector s0 must be dense (pass in full(s0))");
}
if (s0 != NULL && mxIsComplex(s0)) {
mxFree(d); mxFree(k);
mexErrMsgTxt("Initial vector s0 cannot be complex");
}
d->n = getVectorLength(c_mex,"data.c");
d->m = getVectorLength(b_mex,"data.b");
d->b = mxGetPr(b_mex);
d->c = mxGetPr(c_mex);
d->x = x0 ? mxGetPr(x0) : NULL;
d->y = y0 ? mxGetPr(y0) : NULL;
d->s = s0 ? mxGetPr(s0) : NULL;
d->p->ALPHA = getParameterField(params, "ALPHA", d, k);
d->p->MAX_ITERS = (idxint)getParameterField(params, "MAX_ITERS", d, k);
d->p->EPS_ABS = getParameterField(params, "EPS_ABS", d, k);
d->p->CG_MAX_ITS = (idxint)getParameterField(params, "CG_MAX_ITS", d, k);
d->p->CG_TOL = getParameterField(params, "CG_TOL", d, k);
d->p->VERBOSE = (idxint)getParameterField(params, "VERBOSE", d, k);
d->p->NORMALIZE = (idxint)getParameterField(params, "NORMALIZE", d, k);
f_mex = (mxArray *) mxGetField(cone,0,"f");
if(f_mex == NULL) {
mxFree(d); mxFree(k);
mexErrMsgTxt("Cone struct must contain a `f` entry.");
}
k->f = (idxint)*mxGetPr(f_mex);
l_mex = (mxArray *) mxGetField(cone,0,"l");
if(l_mex == NULL) {
mxFree(d); mxFree(k);
mexErrMsgTxt("Cone struct must contain a `l` entry.");
}
k->l = (idxint)*mxGetPr(l_mex);
q_mex = (mxArray *) mxGetField(cone,0,"q");
if(q_mex == NULL) {
mxFree(d); mxFree(k);
mexErrMsgTxt("Cone struct must contain a `q` entry.");
}
q_mex_vals = mxGetPr(q_mex);
k->qsize = getVectorLength(mxGetField(cone,0,"q"), "cone.q");
k->q = mxMalloc(sizeof(idxint)*k->qsize);
for ( i=0; i < k->qsize; i++ ){
k->q[i] = (idxint)q_mex_vals[i];
}
/* int Anz = mxGetNzmax(A_mex); */
d->Ax = (double *)mxGetPr(A_mex);
d->Ai = (long*)mxGetIr(A_mex);
d->Ap = (long*)mxGetJc(A_mex);
/* printConeData(d,k); */
/* printData(d); */
sol = pdos(d,k);
plhs[0] = mxCreateDoubleMatrix(d->n, 1, mxREAL);
mxSetPr(plhs[0], sol->x);
plhs[1] = mxCreateDoubleMatrix(d->m, 1, mxREAL);
mxSetPr(plhs[1], sol->s);
plhs[2] = mxCreateDoubleMatrix(d->m, 1, mxREAL);
mxSetPr(plhs[2], sol->y);
plhs[3] = mxCreateString(sol->status);
mxFree(d->p); mxFree(d); mxFree(k->q); mxFree(k);
return;
}
Vector3D Bevgraf::normalizeVector(Vector3D vector)
{
GLdouble length = getVectorLength(vector);
return initVector3(vector.x / length, vector.y / length, vector.z / length);
}
