void NaviHeightmap::update(const vector<IBox> &walkable, const vector<IBox> &blockers) {
m_level_count = 0;
m_data.clear();
PodArray<IBox> bboxes(walkable.size());
for(int n = 0; n < bboxes.size(); n++) {
IBox bbox = walkable[n];
bboxes[n] = { vmax(bbox.min(), int3(0, 0, 0)), vmin(bbox.max(), int3(m_size.x, 255, m_size.y))};
}
std::sort(bboxes.data(), bboxes.end(), [](const IBox &a, const IBox &b)
{ return a.y() == b.y()? a.x() == b.x()?
a.z() < b.z() : a.x() < b.x() : a.y() < b.y(); } );
for(int n = 0; n < bboxes.size(); n++) {
const IBox &bbox = bboxes[n];
int min_y = bbox.y(), max_y = bbox.ey();
for(int z = 0; z < bbox.depth(); z++)
for(int x = 0; x < bbox.width(); x++) {
int level = 0;
int px = x + bbox.x();
int pz = z + bbox.z();
while(level < m_level_count) {
int value = m_data[index(px, pz, level)];
if(value == invalid_value || value >= min_y - 4)
break;
level++;
}
if(level == m_level_count) {
if(level == max_levels)
continue;
addLevel();
}
m_data[index(px, pz, level)] = max_y;
}
}
for(int n = 0; n < (int)blockers.size(); n++) {
IBox blocker(
vmax(blockers[n].min(), int3(0, 0, 0)),
vmin(blockers[n].max(), int3(m_size.x, 255, m_size.y)));
u8 min_y = max(0, blocker.y() - 4), max_y = blocker.ey();
for(int z = 0; z < blocker.depth(); z++)
for(int x = 0; x < blocker.width(); x++) {
int level = 0;
int px = x + blocker.x();
int pz = z + blocker.z();
while(level < m_level_count) {
u8 &value = m_data[index(px, pz, level++)];
if(value >= min_y && value <= max_y)
value = invalid_value;
}
}
}
}
void View::drawGrid(Renderer2D &out) const {
if(!m_is_visible)
return;
const int2 tile_map_size = m_tile_map.dimensions();
int2 p[4] = {
screenToWorld(m_view_pos + int2(0, 0)),
screenToWorld(m_view_pos + int2(0, m_view_size.y)),
screenToWorld(m_view_pos + int2(m_view_size.x, m_view_size.y)),
screenToWorld(m_view_pos + int2(m_view_size.x, 0)) };
int2 offset = screenToWorld(worldToScreen(int3(0, m_height, 0)));
for(int n = 0; n < 4; n++)
p[n] -= offset;
int2 tmin = vmin(vmin(p[0], p[1]), vmin(p[2], p[3]));
int2 tmax = vmax(vmax(p[0], p[1]), vmax(p[2], p[3]));
IRect box(vmax(tmin, int2(0, 0)), vmin(tmax, tile_map_size));
Color color(255, 255, 255, 64);
for(int x = box.x() - box.x() % m_cell_size; x <= box.ex(); x += m_cell_size)
drawLine(out, int3(x, m_height, box.y()), int3(x, m_height, box.ey()), color);
for(int y = box.y() - box.y() % m_cell_size; y <= box.ey(); y += m_cell_size)
drawLine(out, int3(box.x(), m_height, y), int3(box.ex(), m_height, y), color);
}
//------------------------------------------------------------------------------------------------
Ogre::Vector3 VertexIndexToShape::getSize()
{
const unsigned int vCount = getVertexCount();
if (mBounds == Ogre::Vector3(-1,-1,-1) && vCount > 0)
{
const Ogre::Vector3 * const v = getVertices();
Ogre::Vector3 vmin(v[0]);
Ogre::Vector3 vmax(v[0]);
for(unsigned int j = 1; j < vCount; j++)
{
vmin.x = std::min(vmin.x, v[j].x);
vmin.y = std::min(vmin.y, v[j].y);
vmin.z = std::min(vmin.z, v[j].z);
vmax.x = std::max(vmax.x, v[j].x);
vmax.y = std::max(vmax.y, v[j].y);
vmax.z = std::max(vmax.z, v[j].z);
}
mBounds.x = vmax.x - vmin.x;
mBounds.y = vmax.y - vmin.y;
mBounds.z = vmax.z - vmin.z;
}
return mBounds;
}
std::shared_ptr<FieldHeights> FieldExporter::FieldExporterCSV::loadHeights(std::string filename) {
try {
FileReader br(filename);
int linen = 0;
std::set<double> xset;
std::set<double> yset;
auto poinycomparator = [](const glm::vec2 v1, const glm::vec2 v2) -> bool {
return v1.y < v2.y ? true : v1.x == v2.x && v1.y < v2.y ? true : false;
};
std::map<glm::vec2, glm::vec2, decltype(poinycomparator)> pointmap(poinycomparator);
while (!br.eof()) {
std::string line = br.readLine();
if (linen++ == 0)
continue;
std::vector<std::string> linevalues = Utils::tokenize(line, "\t");
if (linevalues.size() < 3)
continue;
const double x = std::stod(linevalues[0]);
const double y = std::stod(linevalues[1]);
const double vmin = std::stod(linevalues[2]);
const double vmax = std::stod(linevalues[3]);
xset.insert(x);
yset.insert(y);
pointmap[glm::vec2(x, y)] = glm::vec2(vmin, vmax);
}
const std::vector<double> xs = adjustDeltas(doubleSetToSortedArray(xset));
const std::vector<double> ys = adjustDeltas(doubleSetToSortedArray(yset));
yMatrix<double> vmin(xs.size(), ys.size());
yMatrix<double> vmax(xs.size(), ys.size());
for (unsigned y = 0; y<ys.size(); y++)
for (unsigned x = 0; x<xs.size(); x++) {
const auto& v = glm::vec2(xs[x], ys[y]);
if (pointmap.find(v) == pointmap.end()) {
vmin.set(x, y, FieldHeights::UNUSED);
vmax.set(x, y, FieldHeights::UNUSED);
}
else {
vmin.set(x, y, v.x);
vmax.set(x, y, v.y);
}
}
const glm::vec2 spacing(xs[1] - xs[0], ys[1] - ys[0]);
const glm::vec2 center((xs[xs.size() - 1] + xs[0]) / 2, (ys[ys.size() - 1] + ys[0]) / 2);
return std::make_shared<FieldHeights>(vmin, vmax, center, spacing);
}
catch (...) { return std::shared_ptr<FieldHeights>(nullptr); }
}
static gxCanvas *tformCanvas( gxCanvas *c,float m[2][2],int x_handle,int y_handle ){
vec2 v,v0,v1,v2,v3;
float i[2][2];
float dt=1.0f/(m[0][0]*m[1][1]-m[1][0]*m[0][1]);
i[0][0]=dt*m[1][1];i[1][0]=-dt*m[1][0];
i[0][1]=-dt*m[0][1];i[1][1]=dt*m[0][0];
float ox=x_handle,oy=y_handle;
v0.x=-ox;v0.y=-oy; //tl
v1.x=c->getWidth()-ox;v1.y=-oy; //tr
v2.x=c->getWidth()-ox;v2.y=c->getHeight()-oy; //br
v3.x=-ox;v3.y=c->getHeight()-oy; //bl
v0=vrot(m,v0);v1=vrot(m,v1);v2=vrot(m,v2);v3=vrot(m,v3);
float minx=floor( vmin( v0.x,v1.x,v2.x,v3.x ) );
float miny=floor( vmin( v0.y,v1.y,v2.y,v3.y ) );
float maxx=ceil( vmax( v0.x,v1.x,v2.x,v3.x ) );
float maxy=ceil( vmax( v0.y,v1.y,v2.y,v3.y ) );
int iw=maxx-minx,ih=maxy-miny;
gxCanvas *t=gx_graphics->createCanvas( iw,ih,0 );
t->setHandle( -minx,-miny );
t->setMask( c->getMask() );
c->lock();
t->lock();
v.y=miny+.5f;
for( int y=0;y<ih;++v.y,++y ){
v.x=minx+.5f;
for( int x=0;x<iw;++v.x,++x ){
vec2 q=vrot(i,v);
unsigned rgb=filter ? getPixel( c,q.x+ox,q.y+oy ) : c->getPixel( floor(q.x+ox),floor(q.y+oy) );
t->setPixel( x,y,rgb );
}
}
t->unlock();
c->unlock();
return t;
}
//==============================================================================
double LRBSpline2D::evalBasisFunc(double u,
double v) const
//==============================================================================
{
bool u_on_end = (u == umax());
bool v_on_end = (v == vmax());
return
B(degree(XFIXED), u, &kvec(XFIXED)[0], mesh_->knotsBegin(XFIXED), u_on_end)*
B(degree(YFIXED), v, &kvec(YFIXED)[0], mesh_->knotsBegin(YFIXED), v_on_end);
}
void BoundingBox3::merge_with( const Vector3& v )
{
if ( extent == EX_NULL ) {
max = min = v;
extent = EX_FINITE;
} else if ( extent == EX_FINITE ) {
max = vmax( max, v );
min = vmin( min, v );
}
// else if this->extent == infinite, nothing changes
}
void OscMeshCHAI::on_size()
{
m_pMesh->computeBoundaryBox(true);
cVector3d vmin(m_pMesh->getBoundaryMin());
cVector3d vmax(m_pMesh->getBoundaryMax());
cVector3d scale(vmax - vmin);
scale.x = m_size.x / scale.x;
scale.y = m_size.y / scale.y;
scale.z = m_size.z / scale.z;
m_pMesh->scale(scale);
}
Eigen::Vector3d computeShapeExtents(const Shape *shape)
{
if (shape->type == SPHERE)
{
double d = static_cast<const Sphere*>(shape)->radius * 2.0;
return Eigen::Vector3d(d, d, d);
}
else
if (shape->type == BOX)
{
const double* sz = static_cast<const Box*>(shape)->size;
return Eigen::Vector3d(sz[0], sz[1], sz[2]);
}
else
if (shape->type == CYLINDER)
{
double d = static_cast<const Cylinder*>(shape)->radius * 2.0;
return Eigen::Vector3d(d, d, static_cast<const Cylinder*>(shape)->length);
}
else
if (shape->type == CONE)
{
double d = static_cast<const Cone*>(shape)->radius * 2.0;
return Eigen::Vector3d(d, d, static_cast<const Cone*>(shape)->length);
}
else
if (shape->type == MESH)
{
const Mesh *mesh = static_cast<const Mesh*>(shape);
if (mesh->vertex_count > 1)
{
std::vector<double> vmin(3, std::numeric_limits<double>::max());
std::vector<double> vmax(3, -std::numeric_limits<double>::max());
for (unsigned int i = 0; i < mesh->vertex_count ; ++i)
{
unsigned int i3 = i * 3;
for (unsigned int k = 0 ; k < 3 ; ++k)
{
unsigned int i3k = i3 + k;
if (mesh->vertices[i3k] > vmax[k])
vmax[k] = mesh->vertices[i3k];
if (mesh->vertices[i3k] < vmin[k])
vmin[k] = mesh->vertices[i3k];
}
}
return Eigen::Vector3d(vmax[0] - vmin[0], vmax[1] - vmin[1], vmax[2] - vmin[2]);
}
else
return Eigen::Vector3d(0.0, 0.0, 0.0);
}
else
return Eigen::Vector3d(0.0, 0.0, 0.0);
}
void rcCalcBounds(const float* verts, int nv, float* bmin, float* bmax)
{
// Calculate bounding box.
vcopy(bmin, verts);
vcopy(bmax, verts);
for (int i = 1; i < nv; ++i)
{
const float* v = &verts[i*3];
vmin(bmin, v);
vmax(bmax, v);
}
}
/** @brief Ray-AABB intersection test.
* @param origin The origin of the ray to test against.
* @param direction The (unit) direction of the ray to test against.
* @param near A pointer to the near intersection distance (closest).
* @param far A pointer to the far intersection distance (furthest).
* @return Returns \c true if an intersection occurs, and \c false
* otherwise. If this method returns \c false, then the values
* of \c *near and \c *far are indeterminate.
**/
bool Intersect(const Vector& origin, const Vector& direction,
float *near, float *far)
{
Vector bot = (min - origin) / direction;
Vector top = (max - origin) / direction;
Vector tmin = vmin(top, bot);
Vector tmax = vmax(top, bot);
*near = std::max(std::max(tmin.x, tmin.y), tmin.z);
*far = std::min(std::min(tmax.x, tmax.y), tmax.z);
return !(*near > *far) && *far > 0;
}
void scale_prof(float *profile, int nbins, unsigned long *iprofile, float *scale, float *offset) /*includefile*/
{
int j;
float denominator;
*offset=vmin(profile,nbins);
denominator=(vmax(profile,nbins)-*offset);
if (denominator!=0.0)
*scale=65535.0/denominator;
else
*scale=0.0;
for (j=0;j<nbins;j++) {
iprofile[j]=(profile[j]-(*offset))*(*scale);
}
}
//==============================================================================
bool LRBSpline2D::overlaps(double domain[]) const
//==============================================================================
{
// Does it make sense to include equality?
if (domain[0] >= umax())
return false;
if (domain[1] <= umin())
return false;
if (domain[2] >= vmax())
return false;
if (domain[3] <= vmin())
return false;
return true;
}
//==============================================================================
bool LRBSpline2D::overlaps(Element2D *el) const
//==============================================================================
{
// Does it make sense to include equality?
if (el->umin() >= umax())
return false;
if (el->umax() <= umin())
return false;
if (el->vmin() >= vmax())
return false;
if (el->vmax() <= vmin())
return false;
return true;
}
const IBox View::computeCursor(const int2 &start, const int2 &end, const int3 &bbox, int height, int offset) const {
float2 height_off = worldToScreen(float3(0, height, 0));
int3 gbox(cellSize(), 1, cellSize());
int3 start_pos = asXZ((int2)( screenToWorld(float2(start + pos()) - height_off) + float2(0.5f, 0.5f)));
int3 end_pos = asXZ((int2)( screenToWorld(float2(end + pos()) - height_off) + float2(0.5f, 0.5f)));
start_pos.y = end_pos.y = height + offset;
{
int apos1 = start_pos.x % gbox.x;
int apos2 = apos1 - gbox.x + bbox.x;
start_pos.x -= apos1 < gbox.x - apos1 || bbox.x >= gbox.x? apos1 : apos2;
}
{
int apos1 = start_pos.z % gbox.z;
int apos2 = apos1 - gbox.z + bbox.z;
start_pos.z -= apos1 < gbox.z - apos1 || bbox.z >= gbox.z? apos1 : apos2;
}
if(end == start)
end_pos = start_pos;
int3 dir(end_pos.x >= start_pos.x? 1 : -1, 1, end_pos.z >= start_pos.z? 1 : -1);
int3 size(::abs(end_pos.x - start_pos.x), 1, ::abs(end_pos.z - start_pos.z));
size += bbox - int3(1, 1, 1);
size.x -= size.x % bbox.x;
size.z -= size.z % bbox.z;
size = vmax(bbox, size);
if(dir.x < 0)
start_pos.x += bbox.x;
if(dir.z < 0)
start_pos.z += bbox.z;
end_pos = start_pos + dir * size;
if(start_pos.x > end_pos.x) swap(start_pos.x, end_pos.x);
if(start_pos.z > end_pos.z) swap(start_pos.z, end_pos.z);
int2 dims = m_tile_map.dimensions();
start_pos = asXZY(vclamp(start_pos.xz(), int2(0, 0), dims), start_pos.y);
end_pos = asXZY(vclamp( end_pos.xz(), int2(0, 0), dims), end_pos.y);
return IBox(start_pos, end_pos);
}
void Mesh::ComputeBound()
{
// update bounds
if(pos.size() > 0)
{
float3 first = pos[0];
float3 vmin(first);
float3 vmax(first);
for(auto it = pos.begin(); it != pos.end(); ++it)
{
float3 pos = (*it);
vmin = minimize(vmin, pos);
vmax = maximize(vmax, pos);
}
AABB aabb(vmin, vmax);
bounds = aabb;
}
}
//! Check if the given plane intersect the box.
bool intersect(const Plane<Type> & a_plane) const
{
// Empty boxes can cause problems.
if( isEmpty() ) return false;
const Vec3<Type> & pnorm = a_plane.getNormal();
// Use separating axis theorem to test overlap.
Vec3<Type> vmin( pnorm[0] > 0.0 ? m_min[0] : m_max[0],
pnorm[1] > 0.0 ? m_min[1] : m_max[1],
pnorm[2] > 0.0 ? m_min[2] : m_max[2]);
Vec3<Type> vmax( pnorm[0] > 0.0 ? m_max[0] : m_min[0],
pnorm[1] > 0.0 ? m_max[1] : m_min[1],
pnorm[2] > 0.0 ? m_max[2] : m_min[2]);
if(a_plane.isInHalfSpace(vmin)) return false;
if(a_plane.isInHalfSpace(vmax)) return true;
return false;
}
void ComputeSphere(Sphere &sphere, const float* pts, unsigned int numPts, size_t stride)
{
assert( numPts != 0 );
Float3 vmin( pts[0], pts[1], pts[2] );
Float3 vmax( pts[0], pts[1], pts[2] );
for(unsigned int i = 1; i < numPts; ++i)
{
const float* pt = (float*)(((const char*)pts) + i * stride);
for(unsigned int axis = 0; axis < 3; ++axis)
{
if( vmin.v[axis] > pt[axis] )
vmin.v[axis] = pt[axis];
if( vmax.v[axis] < pt[axis] )
vmax.v[axis] = pt[axis];
}
}
sphere.center = (vmin + vmax) * 0.5f;
sphere.radius = 0.0f;
for(unsigned int i = 0; i < numPts; ++i)
{
const float* pt = (float*)(((const char*)pts) + i * stride);
Float3 delta( pt[0], pt[1], pt[2] );
delta -= sphere.center;
float dsq = DotProduct( delta, delta );
if( sphere.radius < dsq )
sphere.radius = dsq;
}
sphere.radius = sqrtf(sphere.radius);
}
bool rcMergePolyMeshes(rcPolyMesh** meshes, const int nmeshes, rcPolyMesh& mesh)
{
if (!nmeshes || !meshes)
return true;
rcTimeVal startTime = rcGetPerformanceTimer();
int* nextVert = 0;
int* firstVert = 0;
unsigned short* vremap = 0;
mesh.nvp = meshes[0]->nvp;
mesh.cs = meshes[0]->cs;
mesh.ch = meshes[0]->ch;
vcopy(mesh.bmin, meshes[0]->bmin);
vcopy(mesh.bmax, meshes[0]->bmax);
int maxVerts = 0;
int maxPolys = 0;
int maxVertsPerMesh = 0;
for (int i = 0; i < nmeshes; ++i)
{
vmin(mesh.bmin, meshes[i]->bmin);
vmax(mesh.bmax, meshes[i]->bmax);
maxVertsPerMesh = rcMax(maxVertsPerMesh, meshes[i]->nverts);
maxVerts += meshes[i]->nverts;
maxPolys += meshes[i]->npolys;
}
mesh.nverts = 0;
mesh.verts = new unsigned short[maxVerts*3];
if (!mesh.verts)
{
if (rcGetLog())
rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.verts' (%d).", maxVerts*3);
return false;
}
mesh.npolys = 0;
mesh.polys = new unsigned short[maxPolys*2*mesh.nvp];
if (!mesh.polys)
{
if (rcGetLog())
rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.polys' (%d).", maxPolys*2*mesh.nvp);
return false;
}
memset(mesh.polys, 0xff, sizeof(unsigned short)*maxPolys*2*mesh.nvp);
mesh.regs = new unsigned short[maxPolys];
if (!mesh.regs)
{
if (rcGetLog())
rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.regs' (%d).", maxPolys);
return false;
}
memset(mesh.regs, 0, sizeof(unsigned short)*maxPolys);
nextVert = new int[maxVerts];
if (!nextVert)
{
if (rcGetLog())
rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'nextVert' (%d).", maxVerts);
goto failure;
}
memset(nextVert, 0, sizeof(int)*maxVerts);
firstVert = new int[VERTEX_BUCKET_COUNT];
if (!firstVert)
{
if (rcGetLog())
rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'firstVert' (%d).", VERTEX_BUCKET_COUNT);
goto failure;
}
for (int i = 0; i < VERTEX_BUCKET_COUNT; ++i)
firstVert[i] = -1;
vremap = new unsigned short[maxVertsPerMesh];
if (!vremap)
{
if (rcGetLog())
rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'vremap' (%d).", maxVertsPerMesh);
goto failure;
}
memset(nextVert, 0, sizeof(int)*maxVerts);
for (int i = 0; i < nmeshes; ++i)
{
const rcPolyMesh* pmesh = meshes[i];
const unsigned short ox = (unsigned short)floorf((pmesh->bmin[0]-mesh.bmin[0])/mesh.cs+0.5f);
const unsigned short oz = (unsigned short)floorf((pmesh->bmin[2]-mesh.bmin[2])/mesh.cs+0.5f);
for (int j = 0; j < pmesh->nverts; ++j)
{
unsigned short* v = &pmesh->verts[j*3];
vremap[j] = addVertex(v[0]+ox, v[1], v[2]+oz,
mesh.verts, firstVert, nextVert, mesh.nverts);
}
for (int j = 0; j < pmesh->npolys; ++j)
{
unsigned short* tgt = &mesh.polys[mesh.npolys*2*mesh.nvp];
unsigned short* src = &pmesh->polys[j*2*mesh.nvp];
mesh.regs[mesh.npolys] = pmesh->regs[j];
mesh.npolys++;
for (int k = 0; k < mesh.nvp; ++k)
{
if (src[k] == 0xffff) break;
tgt[k] = vremap[src[k]];
}
}
}
// Calculate adjacency.
if (!buildMeshAdjacency(mesh.polys, mesh.npolys, mesh.nverts, mesh.nvp))
{
if (rcGetLog())
rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Adjacency failed.");
return false;
}
delete [] firstVert;
delete [] nextVert;
delete [] vremap;
rcTimeVal endTime = rcGetPerformanceTimer();
if (rcGetBuildTimes())
rcGetBuildTimes()->mergePolyMesh += rcGetDeltaTimeUsec(startTime, endTime);
return true;
failure:
delete [] firstVert;
delete [] nextVert;
delete [] vremap;
return false;
}
/** @brief Expands this bounding box to contain an arbitrary bounding box.
* @param b The bounding box this bounding box is to contain.
* @note If \c b is already fully contained in this bounding box, this
* method does nothing.
**/
void ExpandToInclude(const AABB& b)
{
min = vmin(min, b.min);
max = vmax(max, b.max);
extent = max - min;
}
/** @brief Expands this bounding box to contain an arbitrary point.
* @param p The point (as a vector) this bounding box is to contain.
* @note If \c p is already in this bounding box, does nothing.
**/
void ExpandToInclude(const Vector& p)
{
min = vmin(min, p);
max = vmax(max, p);
extent = max - min;
}
void DrawBl()
{
Shader* s = &g_shader[g_curS];
//return;
for(int i=0; i<BUILDINGS; i++)
{
Building* b = &g_building[i];
if(!b->on)
continue;
const BuildingT* t = &g_bltype[b->type];
//const BuildingT* t = &g_bltype[BUILDING_APARTMENT];
Model* m = &g_model[ t->model ];
Vec3f vmin(b->drawpos.x - t->widthx*TILE_SIZE/2, b->drawpos.y, b->drawpos.z - t->widthz*TILE_SIZE/2);
Vec3f vmax(b->drawpos.x + t->widthx*TILE_SIZE/2, b->drawpos.y + (t->widthx+t->widthz)*TILE_SIZE/2, b->drawpos.z + t->widthz*TILE_SIZE/2);
if(!g_frustum.boxin2(vmin.x, vmin.y, vmin.z, vmax.x, vmax.y, vmax.z))
continue;
if(!b->finished)
m = &g_model[ t->cmodel ];
/*
m->draw(0, b->drawpos, 0);
*/
float pitch = 0;
float yaw = 0;
Matrix modelmat;
float radians[] = {(float)DEGTORAD(pitch), (float)DEGTORAD(yaw), 0};
modelmat.translation((const float*)&b->drawpos);
Matrix rotation;
rotation.rotrad(radians);
modelmat.postmult(rotation);
glUniformMatrix4fv(s->m_slot[SSLOT_MODELMAT], 1, 0, modelmat.m_matrix);
Matrix modelview;
#ifdef SPECBUMPSHADOW
modelview.set(g_camview.m_matrix);
#endif
modelview.postmult(modelmat);
glUniformMatrix4fv(s->m_slot[SSLOT_MODELVIEW], 1, 0, modelview.m_matrix);
Matrix mvp;
#if 0
mvp.set(modelview.m_matrix);
mvp.postmult(g_camproj);
#elif 0
mvp.set(g_camproj.m_matrix);
mvp.postmult(modelview);
#else
mvp.set(g_camproj.m_matrix);
mvp.postmult(g_camview);
mvp.postmult(modelmat);
#endif
glUniformMatrix4fv(s->m_slot[SSLOT_MVP], 1, 0, mvp.m_matrix);
Matrix modelviewinv;
Transpose(modelview, modelview);
Inverse2(modelview, modelviewinv);
//Transpose(modelviewinv, modelviewinv);
glUniformMatrix4fv(s->m_slot[SSLOT_NORMALMAT], 1, 0, modelviewinv.m_matrix);
VertexArray* va = &b->drawva;
m->usetex();
glVertexAttribPointer(s->m_slot[SSLOT_POSITION], 3, GL_FLOAT, GL_FALSE, 0, va->vertices);
glVertexAttribPointer(s->m_slot[SSLOT_TEXCOORD0], 2, GL_FLOAT, GL_FALSE, 0, va->texcoords);
if(s->m_slot[SSLOT_NORMAL] != -1)
glVertexAttribPointer(s->m_slot[SSLOT_NORMAL], 3, GL_FLOAT, GL_FALSE, 0, va->normals);
glDrawArrays(GL_TRIANGLES, 0, va->numverts);
}
}
/* NxMxK the size of the data
* comm communicator to use for fft5d
* P0 number of processor in 1st axes (can be null for automatic)
* lin is allocated by fft5d because size of array is only known after planning phase
* rlout2 is only used as intermediate buffer - only returned after allocation to reuse for back transform - should not be used by caller
*/
fft5d_plan fft5d_plan_3d(int NG, int MG, int KG, MPI_Comm comm[2], int flags, t_complex** rlin, t_complex** rlout, t_complex** rlout2, t_complex** rlout3, int nthreads)
{
int P[2], bMaster, prank[2], i, t;
int rNG, rMG, rKG;
int *N0 = 0, *N1 = 0, *M0 = 0, *M1 = 0, *K0 = 0, *K1 = 0, *oN0 = 0, *oN1 = 0, *oM0 = 0, *oM1 = 0, *oK0 = 0, *oK1 = 0;
int N[3], M[3], K[3], pN[3], pM[3], pK[3], oM[3], oK[3], *iNin[3] = {0}, *oNin[3] = {0}, *iNout[3] = {0}, *oNout[3] = {0};
int C[3], rC[3], nP[2];
int lsize;
t_complex *lin = 0, *lout = 0, *lout2 = 0, *lout3 = 0;
fft5d_plan plan;
int s;
/* comm, prank and P are in the order of the decomposition (plan->cart is in the order of transposes) */
#ifdef GMX_MPI
if (GMX_PARALLEL_ENV_INITIALIZED && comm[0] != MPI_COMM_NULL)
{
MPI_Comm_size(comm[0], &P[0]);
MPI_Comm_rank(comm[0], &prank[0]);
}
else
#endif
{
P[0] = 1;
prank[0] = 0;
}
#ifdef GMX_MPI
if (GMX_PARALLEL_ENV_INITIALIZED && comm[1] != MPI_COMM_NULL)
{
MPI_Comm_size(comm[1], &P[1]);
MPI_Comm_rank(comm[1], &prank[1]);
}
else
#endif
{
P[1] = 1;
prank[1] = 0;
}
bMaster = (prank[0] == 0 && prank[1] == 0);
if (debug)
{
fprintf(debug, "FFT5D: Using %dx%d processor grid, rank %d,%d\n",
P[0], P[1], prank[0], prank[1]);
}
if (bMaster)
{
if (debug)
{
fprintf(debug, "FFT5D: N: %d, M: %d, K: %d, P: %dx%d, real2complex: %d, backward: %d, order yz: %d, debug %d\n",
NG, MG, KG, P[0], P[1], (flags&FFT5D_REALCOMPLEX) > 0, (flags&FFT5D_BACKWARD) > 0, (flags&FFT5D_ORDER_YZ) > 0, (flags&FFT5D_DEBUG) > 0);
}
/* The check below is not correct, one prime factor 11 or 13 is ok.
if (fft5d_fmax(fft5d_fmax(lpfactor(NG),lpfactor(MG)),lpfactor(KG))>7) {
printf("WARNING: FFT very slow with prime factors larger 7\n");
printf("Change FFT size or in case you cannot change it look at\n");
printf("http://www.fftw.org/fftw3_doc/Generating-your-own-code.html\n");
}
*/
}
if (NG == 0 || MG == 0 || KG == 0)
{
if (bMaster)
{
printf("FFT5D: FATAL: Datasize cannot be zero in any dimension\n");
}
return 0;
}
rNG = NG; rMG = MG; rKG = KG;
if (flags&FFT5D_REALCOMPLEX)
{
if (!(flags&FFT5D_BACKWARD))
{
NG = NG/2+1;
}
else
{
if (!(flags&FFT5D_ORDER_YZ))
{
MG = MG/2+1;
}
else
{
KG = KG/2+1;
}
}
}
/*for transpose we need to know the size for each processor not only our own size*/
N0 = (int*)malloc(P[0]*sizeof(int)); N1 = (int*)malloc(P[1]*sizeof(int));
M0 = (int*)malloc(P[0]*sizeof(int)); M1 = (int*)malloc(P[1]*sizeof(int));
K0 = (int*)malloc(P[0]*sizeof(int)); K1 = (int*)malloc(P[1]*sizeof(int));
oN0 = (int*)malloc(P[0]*sizeof(int)); oN1 = (int*)malloc(P[1]*sizeof(int));
oM0 = (int*)malloc(P[0]*sizeof(int)); oM1 = (int*)malloc(P[1]*sizeof(int));
oK0 = (int*)malloc(P[0]*sizeof(int)); oK1 = (int*)malloc(P[1]*sizeof(int));
for (i = 0; i < P[0]; i++)
{
#define EVENDIST
#ifndef EVENDIST
oN0[i] = i*ceil((double)NG/P[0]);
oM0[i] = i*ceil((double)MG/P[0]);
oK0[i] = i*ceil((double)KG/P[0]);
#else
oN0[i] = (NG*i)/P[0];
oM0[i] = (MG*i)/P[0];
oK0[i] = (KG*i)/P[0];
#endif
}
for (i = 0; i < P[1]; i++)
{
#ifndef EVENDIST
oN1[i] = i*ceil((double)NG/P[1]);
oM1[i] = i*ceil((double)MG/P[1]);
oK1[i] = i*ceil((double)KG/P[1]);
#else
oN1[i] = (NG*i)/P[1];
oM1[i] = (MG*i)/P[1];
oK1[i] = (KG*i)/P[1];
#endif
}
for (i = 0; i < P[0]-1; i++)
{
N0[i] = oN0[i+1]-oN0[i];
M0[i] = oM0[i+1]-oM0[i];
K0[i] = oK0[i+1]-oK0[i];
}
N0[P[0]-1] = NG-oN0[P[0]-1];
M0[P[0]-1] = MG-oM0[P[0]-1];
K0[P[0]-1] = KG-oK0[P[0]-1];
for (i = 0; i < P[1]-1; i++)
{
N1[i] = oN1[i+1]-oN1[i];
M1[i] = oM1[i+1]-oM1[i];
K1[i] = oK1[i+1]-oK1[i];
}
N1[P[1]-1] = NG-oN1[P[1]-1];
M1[P[1]-1] = MG-oM1[P[1]-1];
K1[P[1]-1] = KG-oK1[P[1]-1];
/* for step 1-3 the local N,M,K sizes of the transposed system
C: contiguous dimension, and nP: number of processor in subcommunicator
for that step */
pM[0] = M0[prank[0]];
oM[0] = oM0[prank[0]];
pK[0] = K1[prank[1]];
oK[0] = oK1[prank[1]];
C[0] = NG;
rC[0] = rNG;
if (!(flags&FFT5D_ORDER_YZ))
{
N[0] = vmax(N1, P[1]);
M[0] = M0[prank[0]];
K[0] = vmax(K1, P[1]);
pN[0] = N1[prank[1]];
iNout[0] = N1;
oNout[0] = oN1;
nP[0] = P[1];
C[1] = KG;
rC[1] = rKG;
N[1] = vmax(K0, P[0]);
pN[1] = K0[prank[0]];
iNin[1] = K1;
oNin[1] = oK1;
iNout[1] = K0;
oNout[1] = oK0;
M[1] = vmax(M0, P[0]);
pM[1] = M0[prank[0]];
oM[1] = oM0[prank[0]];
K[1] = N1[prank[1]];
pK[1] = N1[prank[1]];
oK[1] = oN1[prank[1]];
nP[1] = P[0];
C[2] = MG;
rC[2] = rMG;
iNin[2] = M0;
oNin[2] = oM0;
M[2] = vmax(K0, P[0]);
pM[2] = K0[prank[0]];
oM[2] = oK0[prank[0]];
K[2] = vmax(N1, P[1]);
pK[2] = N1[prank[1]];
oK[2] = oN1[prank[1]];
free(N0); free(oN0); /*these are not used for this order*/
free(M1); free(oM1); /*the rest is freed in destroy*/
}
else
{
N[0] = vmax(N0, P[0]);
M[0] = vmax(M0, P[0]);
K[0] = K1[prank[1]];
pN[0] = N0[prank[0]];
iNout[0] = N0;
oNout[0] = oN0;
nP[0] = P[0];
C[1] = MG;
rC[1] = rMG;
N[1] = vmax(M1, P[1]);
pN[1] = M1[prank[1]];
iNin[1] = M0;
oNin[1] = oM0;
iNout[1] = M1;
oNout[1] = oM1;
M[1] = N0[prank[0]];
pM[1] = N0[prank[0]];
oM[1] = oN0[prank[0]];
K[1] = vmax(K1, P[1]);
pK[1] = K1[prank[1]];
oK[1] = oK1[prank[1]];
nP[1] = P[1];
C[2] = KG;
rC[2] = rKG;
iNin[2] = K1;
oNin[2] = oK1;
M[2] = vmax(N0, P[0]);
pM[2] = N0[prank[0]];
oM[2] = oN0[prank[0]];
K[2] = vmax(M1, P[1]);
pK[2] = M1[prank[1]];
oK[2] = oM1[prank[1]];
free(N1); free(oN1); /*these are not used for this order*/
free(K0); free(oK0); /*the rest is freed in destroy*/
}
N[2] = pN[2] = -1; /*not used*/
/*
Difference between x-y-z regarding 2d decomposition is whether they are
distributed along axis 1, 2 or both
*/
/* int lsize = fmax(N[0]*M[0]*K[0]*nP[0],N[1]*M[1]*K[1]*nP[1]); */
lsize = std::max(N[0]*M[0]*K[0]*nP[0], std::max(N[1]*M[1]*K[1]*nP[1], C[2]*M[2]*K[2]));
/* int lsize = fmax(C[0]*M[0]*K[0],fmax(C[1]*M[1]*K[1],C[2]*M[2]*K[2])); */
if (!(flags&FFT5D_NOMALLOC))
{
snew_aligned(lin, lsize, 32);
snew_aligned(lout, lsize, 32);
if (nthreads > 1)
{
/* We need extra transpose buffers to avoid OpenMP barriers */
snew_aligned(lout2, lsize, 32);
snew_aligned(lout3, lsize, 32);
}
else
{
/* We can reuse the buffers to avoid cache misses */
lout2 = lin;
lout3 = lout;
}
}
else
{
lin = *rlin;
lout = *rlout;
if (nthreads > 1)
{
lout2 = *rlout2;
lout3 = *rlout3;
}
else
{
lout2 = lin;
lout3 = lout;
}
}
plan = (fft5d_plan)calloc(1, sizeof(struct fft5d_plan_t));
if (debug)
{
fprintf(debug, "Running on %d threads\n", nthreads);
}
#ifdef GMX_FFT_FFTW3 /*if not FFTW - then we don't do a 3d plan but instead use only 1D plans */
/* It is possible to use the 3d plan with OMP threads - but in that case it is not allowed to be called from
* within a parallel region. For now deactivated. If it should be supported it has to made sure that
* that the execute of the 3d plan is in a master/serial block (since it contains it own parallel region)
* and that the 3d plan is faster than the 1d plan.
*/
if ((!(flags&FFT5D_INPLACE)) && (!(P[0] > 1 || P[1] > 1)) && nthreads == 1) /*don't do 3d plan in parallel or if in_place requested */
{
int fftwflags = FFTW_DESTROY_INPUT;
FFTW(iodim) dims[3];
int inNG = NG, outMG = MG, outKG = KG;
FFTW_LOCK;
if (!(flags&FFT5D_NOMEASURE))
{
fftwflags |= FFTW_MEASURE;
}
if (flags&FFT5D_REALCOMPLEX)
{
if (!(flags&FFT5D_BACKWARD)) /*input pointer is not complex*/
{
inNG *= 2;
}
else /*output pointer is not complex*/
{
if (!(flags&FFT5D_ORDER_YZ))
{
outMG *= 2;
}
else
{
outKG *= 2;
}
}
}
if (!(flags&FFT5D_BACKWARD))
{
dims[0].n = KG;
dims[1].n = MG;
dims[2].n = rNG;
dims[0].is = inNG*MG; /*N M K*/
dims[1].is = inNG;
dims[2].is = 1;
if (!(flags&FFT5D_ORDER_YZ))
{
dims[0].os = MG; /*M K N*/
dims[1].os = 1;
dims[2].os = MG*KG;
}
else
{
dims[0].os = 1; /*K N M*/
dims[1].os = KG*NG;
dims[2].os = KG;
}
}
else
{
if (!(flags&FFT5D_ORDER_YZ))
{
dims[0].n = NG;
dims[1].n = KG;
dims[2].n = rMG;
dims[0].is = 1;
dims[1].is = NG*MG;
dims[2].is = NG;
dims[0].os = outMG*KG;
dims[1].os = outMG;
dims[2].os = 1;
}
else
{
dims[0].n = MG;
dims[1].n = NG;
dims[2].n = rKG;
dims[0].is = NG;
dims[1].is = 1;
dims[2].is = NG*MG;
dims[0].os = outKG*NG;
dims[1].os = outKG;
dims[2].os = 1;
}
}
#ifdef FFT5D_THREADS
#ifdef FFT5D_FFTW_THREADS
FFTW(plan_with_nthreads)(nthreads);
#endif
#endif
if ((flags&FFT5D_REALCOMPLEX) && !(flags&FFT5D_BACKWARD))
{
plan->p3d = FFTW(plan_guru_dft_r2c)(/*rank*/ 3, dims,
/*howmany*/ 0, /*howmany_dims*/ 0,
(real*)lin, (FFTW(complex) *) lout,
/*flags*/ fftwflags);
}
void CBoxCallback::FillMeshData(Engine::Graphics::IMesh *pMesh)
{
// Declaration
SVertexElement elems[4];
elems[0] = SVertexElement(0, ETYPE_FLOAT3, USG_POSITION, 0);
elems[1] = SVertexElement(sizeof(float) * 3, ETYPE_FLOAT3, USG_NORMAL, 0);
elems[2] = SVertexElement(sizeof(float) * 6, ETYPE_FLOAT2, USG_TEXCOORD, 0);
elems[3] = END_DECLARATION();
pMesh->SetVertexDeclaration(elems);
pMesh->setPrimitiveType(PT_INDEXED_TRIANGLE_LIST);
// Data
const int n_verts = 36;
struct Vert { float data[8]; };
Vert v[] =
{
{ mA[0], mB[1], mA[2], 0, 1, 0 , 0, 1 },
{ mA[0], mB[1], mB[2], 0, 1, 0 , 0, 0 },
{ mB[0], mB[1], mB[2], 0, 1, 0 , 1, 0 },
{ mB[0], mB[1], mB[2], 0, 1, 0 , 1, 0 },
{ mB[0], mB[1], mA[2], 0, 1, 0 , 1, 1 },
{ mA[0], mB[1], mA[2], 0, 1, 0 , 0, 1 },
{ mA[0], mA[1], mA[2], 0, 0, -1 , 0, 1 },
{ mA[0], mB[1], mA[2], 0, 0, -1 , 0, 0 },
{ mB[0], mB[1], mA[2], 0, 0, -1 , 1, 0 },
{ mB[0], mB[1], mA[2], 0, 0, -1 , 1, 0 },
{ mB[0], mA[1], mA[2], 0, 0, -1 , 1, 1 },
{ mA[0], mA[1], mA[2], 0, 0, -1 , 0, 1 },
{ mA[0], mA[1], mB[2], -1, 0, 0 , 0, 1 },
{ mA[0], mB[1], mB[2], -1, 0, 0 , 0, 0 },
{ mA[0], mB[1], mA[2], -1, 0, 0 , 1, 0 },
{ mA[0], mB[1], mA[2], -1, 0, 0 , 1, 0 },
{ mA[0], mA[1], mA[2], -1, 0, 0 , 1, 1 },
{ mA[0], mA[1], mB[2], -1, 0, 0 , 0, 1 },
{ mA[0], mB[1], mB[2], 0, 0, 1 , 1, 0 },
{ mA[0], mA[1], mB[2], 0, 0, 1 , 1, 1 },
{ mB[0], mA[1], mB[2], 0, 0, 1 , 0, 1 },
{ mB[0], mA[1], mB[2], 0, 0, 1 , 0, 1 },
{ mB[0], mB[1], mB[2], 0, 0, 1 , 0, 0 },
{ mA[0], mB[1], mB[2], 0, 0, 1 , 1, 0 },
{ mB[0], mA[1], mA[2], 1, 0, 0 , 0, 1 },
{ mB[0], mB[1], mA[2], 1, 0, 0 , 0, 0 },
{ mB[0], mB[1], mB[2], 1, 0, 0 , 1, 0 },
{ mB[0], mB[1], mB[2], 1, 0, 0 , 1, 0 },
{ mB[0], mA[1], mB[2], 1, 0, 0 , 1, 1 },
{ mB[0], mA[1], mA[2], 1, 0, 0 , 0, 1 },
{ mA[0], mA[1], mB[2], 0, -1, 0 , 0, 1 },
{ mA[0], mA[1], mA[2], 0, -1, 0 , 0, 0 },
{ mB[0], mA[1], mA[2], 0, -1, 0 , 1, 0 },
{ mB[0], mA[1], mA[2], 0, -1, 0 , 1, 0 },
{ mB[0], mA[1], mB[2], 0, -1, 0 , 1, 1 },
{ mA[0], mA[1], mB[2], 0, -1, 0 , 0, 1 },
};
int i[] = {
0,1,2, 3,4,5, 6,7,8, 9,10,11,
12,13,14, 15,16,17, 18,19,20, 21,22,23,
24,25,26, 27,28,29, 30,31,32, 33,34,35, };
IBuffer *vb = pMesh->GetVertexBuffer();
vb->Resize(sizeof(Vert) * n_verts);
void *pData;
vb->Lock(&pData, LOCK_DISCARD);
memcpy(pData, v, sizeof(Vert) * n_verts);
vb->Unlock();
IBuffer *ib = pMesh->GetIndexBuffer();
ib->Resize(sizeof(int) * n_verts);
ib->Lock(&pData, LOCK_DISCARD);
memcpy(pData, i, sizeof(int) * n_verts);
ib->Unlock();
// Subset
IGeometry::TInterval vi(0, n_verts);
IGeometry::TInterval ii(0, n_verts);
pMesh->AddSubset(vi, ii);
VML::Vector3 vmin(mA[0], mA[1], mA[2]);
VML::Vector3 vmax(mB[0], mB[1], mB[2]);
SBoundingVolume bv(vmin, vmax);
pMesh->SetBoundingVolume(bv);
}
main(int argc, char **argv)
{
float tsec,fmhz,peak=1.0,sum,sumsq,sigma,sig2,mean,meansq,chi2,diff;
float *pcopy,n;
int hh,mm,i,j,row,dummy,mbins=64,obins,first=1,idx,nprof=1,display=0,rc2=0,flux=0;
unsigned long *indx;
char line[240], hash, gry[10], message[80];
strcpy(gry," ,:o*@$#");
input=stdin;
if (argc > 1) {
print_version(argv[0],argv[1]);
if (help_required(argv[1])) {
profile_help();
exit(0);
}
i=1;
while (i<argc) {
if (file_exists(argv[i])) {
input=open_file(argv[i],"r");
} else if (strings_equal(argv[i],"-frequency")) {
display=1;
} else if (strings_equal(argv[i],"-chi2")) {
rc2=1;
} else if (strings_equal(argv[i],"-sum")) {
flux=1;
} else if (strings_equal(argv[i],"-p")) {
peak=atof(argv[++i]);
} else {
sprintf(message,"command-line argument %s not recognized...",argv[i]);
error_message(message);
}
i++;
}
}
fgets(line,sizeof(line),input);
sscanf(line,"%c %lf %lf %lf %ld %lf %lf %d",&hash,
&mjdobs,&tstart,&period,&np,&fch1,&refdm,&nbins);
if (nbins > 0) {
while (!feof(input)) {
profile=(float *) malloc(sizeof(float)*nbins);
for (i=0; i<nbins; i++) {
fscanf(input,"%d %f",&dummy,&profile[i]);
}
if (rc2 || flux) {
pcopy=(float *) malloc(sizeof(float)*nbins);
for (i=0;i<nbins;i++) pcopy[i]=profile[i];
indx=(unsigned long *) malloc(sizeof(unsigned long)*nbins);
indexx((unsigned long)nbins,pcopy,indx);
if (flux) {
n=sum=sumsq=0.0;
for (i=0;i<40;i++) {
sum+=profile[i];
sumsq+=profile[i]*profile[i];
n+=1.0;
}
mean=sum/n;
meansq=sumsq/n;
sigma=sqrt(meansq-mean*mean)/sqrt(n-1.0);
n=sum=sumsq=0.0;
for (i=43;i<52;i++) {
sum+=profile[i];
sumsq+=profile[i]*profile[i];
n+=1.0;
}
printf("%f %f\n",sum/n,sigma);
exit(0);
}
n=sum=sumsq=0.0;
for (i=0;i<nbins/2;i++) {
j=indx[i];
sum+=profile[j];
sumsq+=profile[j]*profile[j];
n+=1.0;
}
mean=sum/n;
meansq=sumsq/n;
sigma=sqrt(meansq-mean*mean);
sig2=sigma*sigma;
sumsq=0.0;
for (i=0;i<nbins;i++) {
diff=profile[i]-mean;
sumsq+=diff*diff;
}
//chi2=sumsq/sig2/(float)(nbins-1);
chi2=sumsq/(float)(nbins-1);
printf("%f %f\n",period,chi2);
exit(0);
}
if (nbins>mbins) {
add_channels(profile,nbins,nbins/mbins);
nbins=mbins;
}
pmin=vmin(profile,nbins);
pmax=vmax(profile,nbins)*peak;
prng=pmax-pmin;
for (i=0; i<nbins; i++) profile[i]=19.0*(profile[i]-pmin)/prng+1.0;
for (row=20; row>=1; row--) {
for (i=0; i<nbins; i++) {
if (profile[i]>=(float) row)
printf("#");
else
printf(" ");
}
printf("\n");
}
free(profile);
nbins=0;
fgets(line,sizeof(line),input);
fgets(line,sizeof(line),input);
sscanf(line,"%c %lf %lf %lf %ld %lf %lf %d",&hash,
&mjdobs,&tstart,&period,&np,&fch1,&refdm,&nbins);
if (nbins<=0) break;
}
} else {
while (!feof(input)) {
sscanf(line,"#START %d %f %f",&nbins,&tsec,&fmhz);
if (nbins <=0) break;
profile=(float *) malloc(sizeof(float)*nbins);
for (i=0; i<nbins; i++) fscanf(input,"%d %f",&dummy,&profile[i]);
fgets(line,sizeof(line),input);
fgets(line,sizeof(line),input);
if (nbins>mbins) {
obins=nbins;
add_channels(profile,nbins,nbins/mbins);
nbins=mbins;
}
if (first) {
pmin=vmin(profile,nbins);
pmax=vmax(profile,nbins)*peak;
prng=pmax-pmin;
/*first=0;*/
}
if (display) {
printf("|%04d|%11.6f|",nprof,fmhz);
} else {
hh=(int) tsec/3600.0;
tsec-=(float) hh*3600.0;
mm=(int) tsec/60.0;
tsec-=(float) mm*60.0;
printf("|%04d|%02d:%02d:%02d|",nprof,hh,mm,(int)tsec);
}
for (i=0; i<nbins; i++) {
idx=(int) (9.0*(profile[i]-pmin)/prng);
if (idx<0) idx=0;
if (idx>9) idx=9;
putchar(gry[idx]);
}
puts("|");
free(profile);
fgets(line,sizeof(line),input);
nbins=0;
nprof++;
}
}
}
static bool buildPolyDetail(const float* in, const int nin, unsigned short reg,
const float sampleDist, const float sampleMaxError,
const rcCompactHeightfield& chf, const rcHeightPatch& hp,
float* verts, int& nverts, rcIntArray& tris,
rcIntArray& edges, rcIntArray& idx, rcIntArray& samples)
{
static const int MAX_VERTS = 256;
static const int MAX_EDGE = 64;
float edge[(MAX_EDGE+1)*3];
nverts = 0;
for (int i = 0; i < nin; ++i)
vcopy(&verts[i*3], &in[i*3]);
nverts = nin;
const float ics = 1.0f/chf.cs;
// Tesselate outlines.
// This is done in separate pass in order to ensure
// seamless height values across the ply boundaries.
if (sampleDist > 0)
{
for (int i = 0, j = nin-1; i < nin; j=i++)
{
const float* vj = &in[j*3];
const float* vi = &in[i*3];
// Make sure the segments are always handled in same order
// using lexological sort or else there will be seams.
if (fabsf(vj[0]-vi[0]) < 1e-6f)
{
if (vj[2] > vi[2])
rcSwap(vj,vi);
}
else
{
if (vj[0] > vi[0])
rcSwap(vj,vi);
}
// Create samples along the edge.
float dx = vi[0] - vj[0];
float dy = vi[1] - vj[1];
float dz = vi[2] - vj[2];
float d = sqrtf(dx*dx + dz*dz);
int nn = 1 + (int)floorf(d/sampleDist);
if (nn > MAX_EDGE) nn = MAX_EDGE;
if (nverts+nn >= MAX_VERTS)
nn = MAX_VERTS-1-nverts;
for (int k = 0; k <= nn; ++k)
{
float u = (float)k/(float)nn;
float* pos = &edge[k*3];
pos[0] = vj[0] + dx*u;
pos[1] = vj[1] + dy*u;
pos[2] = vj[2] + dz*u;
pos[1] = chf.bmin[1] + getHeight(pos, chf.bmin, ics, hp)*chf.ch;
}
// Simplify samples.
int idx[MAX_EDGE] = {0,nn};
int nidx = 2;
for (int k = 0; k < nidx-1; )
{
const int a = idx[k];
const int b = idx[k+1];
const float* va = &edge[a*3];
const float* vb = &edge[b*3];
// Find maximum deviation along the segment.
float maxd = 0;
int maxi = -1;
for (int m = a+1; m < b; ++m)
{
float d = distancePtSeg(&edge[m*3],va,vb);
if (d > maxd)
{
maxd = d;
maxi = m;
}
}
// If the max deviation is larger than accepted error,
// add new point, else continue to next segment.
if (maxi != -1 && maxd > rcSqr(sampleMaxError))
{
for (int m = nidx; m > k; --m)
idx[m] = idx[m-1];
idx[k+1] = maxi;
nidx++;
}
else
{
++k;
}
}
// Add new vertices.
for (int k = 1; k < nidx-1; ++k)
{
vcopy(&verts[nverts*3], &edge[idx[k]*3]);
nverts++;
}
}
}
// Tesselate the base mesh.
edges.resize(0);
tris.resize(0);
idx.resize(0);
delaunay(nverts, verts, idx, tris, edges);
if (sampleDist > 0)
{
// Create sample locations in a grid.
float bmin[3], bmax[3];
vcopy(bmin, in);
vcopy(bmax, in);
for (int i = 1; i < nin; ++i)
{
vmin(bmin, &in[i*3]);
vmax(bmax, &in[i*3]);
}
int x0 = (int)floorf(bmin[0]/sampleDist);
int x1 = (int)ceilf(bmax[0]/sampleDist);
int z0 = (int)floorf(bmin[2]/sampleDist);
int z1 = (int)ceilf(bmax[2]/sampleDist);
samples.resize(0);
for (int z = z0; z < z1; ++z)
{
for (int x = x0; x < x1; ++x)
{
float pt[3];
pt[0] = x*sampleDist;
pt[2] = z*sampleDist;
// Make sure the samples are not too close to the edges.
if (distToPoly(nin,in,pt) > -sampleDist/2) continue;
samples.push(x);
samples.push(getHeight(pt, chf.bmin, ics, hp));
samples.push(z);
}
}
// Add the samples starting from the one that has the most
// error. The procedure stops when all samples are added
// or when the max error is within treshold.
const int nsamples = samples.size()/3;
for (int iter = 0; iter < nsamples; ++iter)
{
// Find sample with most error.
float bestpt[3];
float bestd = 0;
for (int i = 0; i < nsamples; ++i)
{
float pt[3];
pt[0] = samples[i*3+0]*sampleDist;
pt[1] = chf.bmin[1] + samples[i*3+1]*chf.ch;
pt[2] = samples[i*3+2]*sampleDist;
float d = distToTriMesh(pt, verts, nverts, &tris[0], tris.size()/4);
if (d < 0) continue; // did not hit the mesh.
if (d > bestd)
{
bestd = d;
vcopy(bestpt,pt);
}
}
// If the max error is within accepted threshold, stop tesselating.
if (bestd <= sampleMaxError)
break;
// Add the new sample point.
vcopy(&verts[nverts*3],bestpt);
nverts++;
// Create new triangulation.
// TODO: Incremental add instead of full rebuild.
edges.resize(0);
tris.resize(0);
idx.resize(0);
delaunay(nverts, verts, idx, tris, edges);
if (nverts >= MAX_VERTS)
break;
}
}
return true;
}
/*
* filter one vertex to enlarge this bounding box.
* \@param vertex A vertex which need to be filtered.
*/
void filter_vertex(const Vector3& vertex)
{
left_bottom_front_vertex = vmin(left_bottom_front_vertex, vertex);
right_top_back_vertex = vmax(right_top_back_vertex , vertex);
}
void rcMarkConvexPolyArea(const float* verts, const int nverts,
const float hmin, const float hmax, unsigned char areaId,
rcCompactHeightfield& chf)
{
float bmin[3], bmax[3];
vcopy(bmin, verts);
vcopy(bmax, verts);
for (int i = 1; i < nverts; ++i)
{
vmin(bmin, &verts[i*3]);
vmax(bmax, &verts[i*3]);
}
bmin[1] = hmin;
bmax[1] = hmax;
int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
if (maxx < 0) return;
if (minx >= chf.width) return;
if (maxz < 0) return;
if (minz >= chf.height) return;
if (minx < 0) minx = 0;
if (maxx >= chf.width) maxx = chf.width-1;
if (minz < 0) minz = 0;
if (maxz >= chf.height) maxz = chf.height-1;
// TODO: Optimize.
for (int z = minz; z <= maxz; ++z)
{
for (int x = minx; x <= maxx; ++x)
{
const rcCompactCell& c = chf.cells[x+z*chf.width];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
rcCompactSpan& s = chf.spans[i];
if ((int)s.y >= miny && (int)s.y <= maxy)
{
if (areaId < chf.areas[i])
{
float p[3];
p[0] = chf.bmin[0] + (x+0.5f)*chf.cs;
p[1] = 0;
p[2] = chf.bmin[2] + (z+0.5f)*chf.cs;
if (pointInPoly(nverts, verts, p))
{
chf.areas[i] = areaId;
}
}
}
}
}
}
}
// This function creates a KD tree with the given
// points, array, and length
int KDTree::create(float *setpoints, int setnpoints, int setndim,
bool setCopy, struct KDTreeNode *setNodeMem)
{
ndim = setndim;
npoints = setnpoints;
typedef int *intptr;
// Copy the points from the original array, if necessary
copyPoints = setCopy;
if (copyPoints) {
if(points) delete[] points;
points = new float[ndim*npoints];
memcpy(points,setpoints,sizeof(float)*ndim*npoints);
}
// If we are not copying, just set the pointer
else
points = setpoints;
// Allocate some arrays;
if (workArr)
delete[]workArr;
workArr = new int[npoints];
if(!setNodeMem) {
if(m_Root) delete[] m_Root;
m_Root = new struct KDTreeNode[npoints*2+1];
nodeMemAlloc = true;
}
else {
m_Root = setNodeMem;
nodeMemAlloc = false;
}
nodeMemCnt = 0;
// Alocate array used for indexing
if(intArrMem) delete[] intArrMem;
intArrMem =
new int[(int)((float)(npoints+4)*
ceil(log((double)npoints)/log(2.0)))];
intArrMemCnt = 0;
// Create the "sortidx" array by
// sorting the range tree points on each dimension
int **sortidx = new intptr[ndim];
if(verbosity>1)
logmsg("KDTree: Sorting points\n");
float imin[3];
float imax[3];
imin[0] = imin[1] = imin[2] = 999999.f;
imax[0] = imax[1] = imax[2] = -999999.f;
for (int i = 0; i < ndim; i++) {
// Initialize the sortidx array for this
// dimension
sortidx[i] = new int[npoints];
// Initialize the "tmp" array for the sort
int *tmp = new int[npoints];
for (int j = 0; j < npoints; j++)
tmp[j] = j;
// Sort the points on dimension i, putting
// indexes in array "tmp"
heapsort(i,tmp,npoints);
// sortidx is actually the inverse of the
// index sorts
for (int j = 0; j < npoints; j++)
{
sortidx[i][tmp[j]] = j;
imin[i] = min( points[ j*3 + i ], imin[ i ] );
imax[i] = max( points[ j*3 + i ], imax[ i ] );
}
delete[] tmp;
}
if(verbosity > 1)
logmsg("KDTree: Done sorting points\n");
// Create an initial list of points that references
// all the points
int *pidx = new int[npoints];
for (int i = 0; i < npoints; i++)
pidx[i] = i;
// Build a KD Tree
AABB extents;
Vec3 vmin( imin[0] ,
imin[1],
imin[2] );
Vec3 vmax( imax[0] ,
imax[1],
imax[2] );
OutputVector( vmin, "VMin");
OutputVector( vmax, "VMax");
extents.Set( vmin,vmax );
m_Root->bounds.Set( vmin, vmax );
//add objects to this node
if( m_NodeCreationCallBack )
{
(*m_NodeCreationCallBack)( m_Root );
}
build_kdtree(sortidx, // array of sort values
0, // The current dimension
pidx, npoints, extents); // The list of points
// Delete the sort index
for (int i = 0; i < ndim; i++)
delete[]sortidx[i];
delete[] sortidx;
// Delete the initial list of points
delete[] pidx;
// Delete the sort arrays
delete[] intArrMem;
// delete the work array
if(workArr) {
delete[] workArr;
workArr = (int *)NULL;
}
if(verbosity > 1)
logmsg("KDTree: Done creating tree\n");
return 0;
} // end of create
static void
cpArbiterApplyImpulse_NEON(cpArbiter *arb)
{
cpBody *a = arb->body_a;
cpBody *b = arb->body_b;
cpFloatx2_t surface_vr = vld((cpFloat_t *)&arb->surface_vr);
cpFloatx2_t n = vld((cpFloat_t *)&arb->n);
cpFloat_t friction = arb->u;
int numContacts = arb->count;
struct cpContact *contacts = arb->contacts;
for(int i=0; i<numContacts; i++) {
struct cpContact *con = contacts + i;
cpFloatx2_t r1 = vld((cpFloat_t *)&con->r1);
cpFloatx2_t r2 = vld((cpFloat_t *)&con->r2);
cpFloatx2_t perp = vmake(-1.0, 1.0);
cpFloatx2_t r1p = vmul(vrev(r1), perp);
cpFloatx2_t r2p = vmul(vrev(r2), perp);
cpFloatx2_t vBias_a = vld((cpFloat_t *)&a->v_bias);
cpFloatx2_t vBias_b = vld((cpFloat_t *)&b->v_bias);
cpFloatx2_t wBias = vmake(a->w_bias, b->w_bias);
cpFloatx2_t vb1 = vadd(vBias_a, vmul_n(r1p, vget_lane(wBias, 0)));
cpFloatx2_t vb2 = vadd(vBias_b, vmul_n(r2p, vget_lane(wBias, 1)));
cpFloatx2_t vbr = vsub(vb2, vb1);
cpFloatx2_t v_a = vld((cpFloat_t *)&a->v);
cpFloatx2_t v_b = vld((cpFloat_t *)&b->v);
cpFloatx2_t w = vmake(a->w, b->w);
cpFloatx2_t v1 = vadd(v_a, vmul_n(r1p, vget_lane(w, 0)));
cpFloatx2_t v2 = vadd(v_b, vmul_n(r2p, vget_lane(w, 1)));
cpFloatx2_t vr = vsub(v2, v1);
cpFloatx2_t vbn_vrn = vpadd(vmul(vbr, n), vmul(vr, n));
cpFloatx2_t v_offset = vmake(con->bias, -con->bounce);
cpFloatx2_t jOld = vmake(con->jBias, con->jnAcc);
cpFloatx2_t jbn_jn = vmul_n(vsub(v_offset, vbn_vrn), con->nMass);
jbn_jn = vmax(vadd(jOld, jbn_jn), vdup_n(0.0));
cpFloatx2_t jApply = vsub(jbn_jn, jOld);
cpFloatx2_t t = vmul(vrev(n), perp);
cpFloatx2_t vrt_tmp = vmul(vadd(vr, surface_vr), t);
cpFloatx2_t vrt = vpadd(vrt_tmp, vrt_tmp);
cpFloatx2_t jtOld = {};
jtOld = vset_lane(con->jtAcc, jtOld, 0);
cpFloatx2_t jtMax = vrev(vmul_n(jbn_jn, friction));
cpFloatx2_t jt = vmul_n(vrt, -con->tMass);
jt = vmax(vneg(jtMax), vmin(vadd(jtOld, jt), jtMax));
cpFloatx2_t jtApply = vsub(jt, jtOld);
cpFloatx2_t i_inv = vmake(-a->i_inv, b->i_inv);
cpFloatx2_t nperp = vmake(1.0, -1.0);
cpFloatx2_t jBias = vmul_n(n, vget_lane(jApply, 0));
cpFloatx2_t jBiasCross = vmul(vrev(jBias), nperp);
cpFloatx2_t biasCrosses = vpadd(vmul(r1, jBiasCross), vmul(r2, jBiasCross));
wBias = vadd(wBias, vmul(i_inv, biasCrosses));
vBias_a = vsub(vBias_a, vmul_n(jBias, a->m_inv));
vBias_b = vadd(vBias_b, vmul_n(jBias, b->m_inv));
cpFloatx2_t j = vadd(vmul_n(n, vget_lane(jApply, 1)), vmul_n(t, vget_lane(jtApply, 0)));
cpFloatx2_t jCross = vmul(vrev(j), nperp);
cpFloatx2_t crosses = vpadd(vmul(r1, jCross), vmul(r2, jCross));
w = vadd(w, vmul(i_inv, crosses));
v_a = vsub(v_a, vmul_n(j, a->m_inv));
v_b = vadd(v_b, vmul_n(j, b->m_inv));
// TODO would moving these earlier help pipeline them better?
vst((cpFloat_t *)&a->v_bias, vBias_a);
vst((cpFloat_t *)&b->v_bias, vBias_b);
vst_lane((cpFloat_t *)&a->w_bias, wBias, 0);
vst_lane((cpFloat_t *)&b->w_bias, wBias, 1);
vst((cpFloat_t *)&a->v, v_a);
vst((cpFloat_t *)&b->v, v_b);
vst_lane((cpFloat_t *)&a->w, w, 0);
vst_lane((cpFloat_t *)&b->w, w, 1);
vst_lane((cpFloat_t *)&con->jBias, jbn_jn, 0);
vst_lane((cpFloat_t *)&con->jnAcc, jbn_jn, 1);
vst_lane((cpFloat_t *)&con->jtAcc, jt, 0);
}
}
