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;
}
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);
}
}
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);
}
}
/** @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;
}
//==============================================================================
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;
}
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);
}
// 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
void DepthBuffer::binTriangles4Simd(vec4f vertices[12],uint32 count) {
enum { kNumLanes = 4 };
VecF32Soa transformedPos[3];
gather4Simd(transformedPos,(VecF32*)vertices);
VecS32 vertexX[3],vertexY[3];
VecF32 vertexZ[3];
for(int i = 0;i<3;i++){
//Convert the floating point coordinates to integer screen space coordinates.
//NB: truncate
vertexX[i] = ftoi(transformedPos[i].x);
vertexY[i] = ftoi(transformedPos[i].y);
vertexZ[i] = transformedPos[i].z;
}
//Compute triangle area.
VecS32 area = (vertexX[1] - vertexX[0]) * (vertexY[2] - vertexY[0]) - (vertexX[0] - vertexX[2]) * (vertexY[0] - vertexY[1]);
VecF32 oneOverArea = VecF32(1.0f)/itof(area);
//Setup Z for interpolation
vertexZ[1] = (vertexZ[1] - vertexZ[0]) * oneOverArea;
vertexZ[2] = (vertexZ[2] - vertexZ[0]) * oneOverArea;
//Find bounding box for the screen space triangle
VecS32 zero = VecS32(0);
VecS32 minX = vmax( vmin(vmin(vertexX[0],vertexX[1]),vertexX[2]), zero);
VecS32 maxX = vmin( vmax(vmax(vertexX[0],vertexX[1]),vertexX[2]), VecS32(size_.x-1) );
VecS32 minY = vmax( vmin(vmin(vertexY[0],vertexY[1]),vertexY[2]), zero);
VecS32 maxY = vmin( vmax(vmax(vertexY[0],vertexY[1]),vertexY[2]), VecS32(size_.y-1) );
uint32 numLanes = std::min(count,uint32(kNumLanes));
for(uint32 i =0;i<numLanes;++i){
//Skip triangle if the area is zero
if(area.lane[i] <= 0) continue;
float oneOverW[3];
for(int j = 0;j<3;++j){
oneOverW[j] = transformedPos[j].w.lane[i];
}
// Reject the triangle if any of its verts is behind the nearclip plane
if(oneOverW[0] == 0.0f || oneOverW[1] == 0.0f || oneOverW[2] == 0.0f) continue;
//Convert bounding box in terms of pixels to bounding box in terms of tiles.
int32 tileMinX = minX.lane[i]/tileSize_.x;//std::max(minX.lane[i]/tileSize_.x,0);
int32 tileMaxX = maxX.lane[i]/tileSize_.x;//std::min(maxX.lane[i]/tileSize_.x,tileCount_.x);
int32 tileMinY = minY.lane[i]/tileSize_.y;//std::max(minY.lane[i]/tileSize_.y,0);
int32 tileMaxY = maxY.lane[i]/tileSize_.y;//std::min(maxY.lane[i]/tileSize_.y,tileCount_.y);
for(;tileMinY <= tileMaxY;tileMinY++){
auto tileIndex = tileMinX + tileMinY*tileCount_.x;
for(auto x = tileMinX; x<= tileMaxX; x++,tileIndex++){
auto count = tileTriangleCount_[tileIndex];
if(count >= kMaxTrianglesPerTile) continue;
tileTriangleCount_[tileIndex]++;
BinnedTriangle& triangle =*( triangleBins_ + count + x*kMaxTrianglesPerTile + tileMinY*tileCount_.x*kMaxTrianglesPerTile);
triangle.v[0].x = vertexX[0].lane[i];
triangle.v[0].y = vertexY[0].lane[i];
triangle.v[1].x = vertexX[1].lane[i];
triangle.v[1].y = vertexY[1].lane[i];
triangle.v[2].x = vertexX[2].lane[i];
triangle.v[2].y = vertexY[2].lane[i];
triangle.z[0] = vertexZ[0].lane[i];
triangle.z[1] = vertexZ[1].lane[i];
triangle.z[2] = vertexZ[2].lane[i];
} }
}
}
void DepthBuffer::rasterizeTile(int32 x,int32 y,uint32 pass) {
if(pass == 0){
//init tile(clear depth).
//auto tilePixels = data_ + x*tileSize_.x*tileSize_.y + (y*tileSize_.x*tileSize_.y)*tileCount_.x;
//clearDepth(tilePixels,tileSize_.x*tileSize_.y,1.0f);
}
if(mode_ == kModeDepthPackedQuads){
rasterizeTile2x2(x,y,pass);
return;
}
auto tileIndex = x + y*tileCount_.x;
auto count = tileTriangleCount_[tileIndex];
tileTriangleCount_[tileIndex] = 0;
auto faces = triangleBins_ + x*kMaxTrianglesPerTile + y*tileCount_.x*kMaxTrianglesPerTile;
vec2i tilePos(x*tileSize_.x,y*tileSize_.y);
vec2i tileEnd(tilePos + tileSize_);
#ifdef ARPHEG_ARCH_X86
enum { kNumLanes = 4 };
//Flush denormals to zero
//_mm_setcsr( _mm_getcsr() | 0x8040 );
VecS32 colOffset(0, 1, 0, 1);
VecS32 rowOffset(0, 0, 1, 1);
//Process the 4 binned triangles at a time
VecS32 vertexX[3];
VecS32 vertexY[3];
VecF32 vertexZ[4];
VecS32 tileMinXSimd(tilePos.x);
VecS32 tileMaxXSimd(tilePos.x+tileSize_.x-1);
VecS32 tileMinYSimd(tilePos.y);
VecS32 tileMaxYSimd(tilePos.y+tileSize_.y-1);
for(uint32 i = 0;i<count;i += kNumLanes){
uint32 numSimdTris = std::min(uint32(kNumLanes),count-i);
auto f = faces+i;
for(uint32 ii = 0;ii< numSimdTris;++ii){
vertexX[0].lane[ii] = f[ii].v[0].x;
vertexY[0].lane[ii] = f[ii].v[0].y;
vertexX[1].lane[ii] = f[ii].v[1].x;
vertexY[1].lane[ii] = f[ii].v[1].y;
vertexX[2].lane[ii] = f[ii].v[2].x;
vertexY[2].lane[ii] = f[ii].v[2].y;
vertexZ[ii] = VecF32(f[ii].z[0],f[ii].z[1],f[ii].z[2],0.0f);
}
// Fab(x, y) = Ax + By + C = 0
// Fab(x, y) = (ya - yb)x + (xb - xa)y + (xa * yb - xb * ya) = 0
// Compute A = (ya - yb) for the 3 line segments that make up each triangle
VecS32 A0 = vertexY[1] - vertexY[2];
VecS32 A1 = vertexY[2] - vertexY[0];
VecS32 A2 = vertexY[0] - vertexY[1];
// Compute B = (xb - xa) for the 3 line segments that make up each triangle
VecS32 B0 = vertexX[2] - vertexX[1];
VecS32 B1 = vertexX[0] - vertexX[2];
VecS32 B2 = vertexX[1] - vertexX[0];
// Compute C = (xa * yb - xb * ya) for the 3 line segments that make up each triangle
VecS32 C0 = vertexX[1] * vertexY[2] - vertexX[2] * vertexY[1];
VecS32 C1 = vertexX[2] * vertexY[0] - vertexX[0] * vertexY[2];
VecS32 C2 = vertexX[0] * vertexY[1] - vertexX[1] * vertexY[0];
// Use bounding box traversal strategy to determine which pixels to rasterize
VecS32 minX = vmax(vmin(vmin(vertexX[0], vertexX[1]), vertexX[2]), tileMinXSimd);
VecS32 maxX = vmin(vmax(vmax(vertexX[0], vertexX[1]), vertexX[2]), tileMaxXSimd);
VecS32 minY = vmax(vmin(vmin(vertexY[0], vertexY[1]), vertexY[2]), tileMinYSimd);
VecS32 maxY = vmin(vmax(vmax(vertexY[0], vertexY[1]), vertexY[2]), tileMaxYSimd);
//Rasterize each triangle individually
for(uint32 lane = 0;lane < numSimdTris;++lane){
float zz[3] = { vertexZ[lane].lane[0],vertexZ[lane].lane[1],vertexZ[lane].lane[2] };
int32 a0 = A0.lane[lane];
int32 a1 = A1.lane[lane];
int32 a2 = A2.lane[lane];
int32 b0 = B0.lane[lane];
int32 b1 = B1.lane[lane];
int32 b2 = B2.lane[lane];
int32 minx = minX.lane[lane];
int32 maxx = maxX.lane[lane];
int32 miny = minY.lane[lane];
int32 maxy = maxY.lane[lane];
auto w0_row = a0 * minx + b0 * miny + C0.lane[lane];
auto w1_row = a1 * minx + b1 * miny + C1.lane[lane];
auto w2_row = a2 * minx + b2 * miny + C2.lane[lane];
float* tilePixels = data_ + tilePos.x*tileSize_.y + (tilePos.y*tileSize_.x)*tileCount_.x;
int32 idx2 = minx-tilePos.x + (miny - tilePos.y)*tileSize_.x;
int32 spanx = maxx-minx;
for(int32 endIdx2 = idx2+(tileSize_.x)*(maxy-miny);idx2<=endIdx2;idx2+=tileSize_.x){
auto w0 = w0_row;
auto w1 = w1_row;
auto w2 = w2_row;
auto idx = idx2;
for(int32 endIdx = idx+spanx;idx<=endIdx;++idx){
auto mask = w0|w1|w2;
if(mask >= 0){
float betaf = float(w1);
float gamaf = float(w2);
float depth = zz[0] + betaf*zz[1] + gamaf*zz[2];
auto d = tilePixels[idx];
d = depth<d?depth:d;
tilePixels[idx] = d;
}
w0+=a0;
w1+=a1;
w2+=a2;
}
w0_row += b0;
w1_row += b1;
w2_row += b2;
}
}
}
#else
for(uint32 i = 0;i<count;i ++){
drawTriangle(faces[i],tilePos);
}
#endif
}
/** @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;
}
//Rasterize 4 pixels at once
void DepthBuffer::rasterizeTile2x2(int32 x,int32 y,uint32 pass) {
auto tileIndex = x + y*tileCount_.x;
auto count = tileTriangleCount_[tileIndex];
tileTriangleCount_[tileIndex] = 0;
auto faces = triangleBins_ + x*kMaxTrianglesPerTile + y*tileCount_.x*kMaxTrianglesPerTile;
vec2i tilePos(x*tileSize_.x,y*tileSize_.y);
vec2i tileEnd(tilePos + tileSize_);
#ifdef ARPHEG_ARCH_X86
enum { kNumLanes = 4 };
//Flush denormals to zero
_mm_setcsr( _mm_getcsr() | 0x8040 );
VecS32 colOffset(0, 1, 0, 1);
VecS32 rowOffset(0, 0, 1, 1);
//Process the 4 binned triangles at a time
VecS32 vertexX[3];
VecS32 vertexY[3];
VecF32 vertexZ[4];
VecS32 tileMinXSimd(tilePos.x);
VecS32 tileMaxXSimd(tilePos.x+tileSize_.x-2);
VecS32 tileMinYSimd(tilePos.y);
VecS32 tileMaxYSimd(tilePos.y+tileSize_.y-2);
for(uint32 i = 0;i<count;i += kNumLanes){
uint32 numSimdTris = std::min(uint32(kNumLanes),count-i);
auto f = faces+i;
for(uint32 ii = 0;ii< numSimdTris;++ii){
vertexX[0].lane[ii] = f[ii].v[0].x;
vertexY[0].lane[ii] = f[ii].v[0].y;
vertexX[1].lane[ii] = f[ii].v[1].x;
vertexY[1].lane[ii] = f[ii].v[1].y;
vertexX[2].lane[ii] = f[ii].v[2].x;
vertexY[2].lane[ii] = f[ii].v[2].y;
vertexZ[ii] = VecF32(f[ii].z[0],f[ii].z[1],f[ii].z[2],0.0f);
}
// Fab(x, y) = Ax + By + C = 0
// Fab(x, y) = (ya - yb)x + (xb - xa)y + (xa * yb - xb * ya) = 0
// Compute A = (ya - yb) for the 3 line segments that make up each triangle
VecS32 A0 = vertexY[1] - vertexY[2];
VecS32 A1 = vertexY[2] - vertexY[0];
VecS32 A2 = vertexY[0] - vertexY[1];
// Compute B = (xb - xa) for the 3 line segments that make up each triangle
VecS32 B0 = vertexX[2] - vertexX[1];
VecS32 B1 = vertexX[0] - vertexX[2];
VecS32 B2 = vertexX[1] - vertexX[0];
// Compute C = (xa * yb - xb * ya) for the 3 line segments that make up each triangle
VecS32 C0 = vertexX[1] * vertexY[2] - vertexX[2] * vertexY[1];
VecS32 C1 = vertexX[2] * vertexY[0] - vertexX[0] * vertexY[2];
VecS32 C2 = vertexX[0] * vertexY[1] - vertexX[1] * vertexY[0];
// Use bounding box traversal strategy to determine which pixels to rasterize
VecS32 minX = vmax(vmin(vmin(vertexX[0], vertexX[1]), vertexX[2]), tileMinXSimd) & VecS32(~1);
VecS32 maxX = vmin(vmax(vmax(vertexX[0], vertexX[1]), vertexX[2]), tileMaxXSimd);
VecS32 minY = vmax(vmin(vmin(vertexY[0], vertexY[1]), vertexY[2]), tileMinYSimd) & VecS32(~1);
VecS32 maxY = vmin(vmax(vmax(vertexY[0], vertexY[1]), vertexY[2]), tileMaxYSimd);
//Rasterize each triangle individually
for(uint32 lane = 0;lane < numSimdTris;++lane){
//Rasterize in 2x2 quads.
VecF32 zz[3];
zz[0] = VecF32(vertexZ[lane].lane[0]);
zz[1] = VecF32(vertexZ[lane].lane[1]);
zz[2] = VecF32(vertexZ[lane].lane[2]);
VecS32 a0(A0.lane[lane]);
VecS32 a1(A1.lane[lane]);
VecS32 a2(A2.lane[lane]);
VecS32 b0(B0.lane[lane]);
VecS32 b1(B1.lane[lane]);
VecS32 b2(B2.lane[lane]);
int32 minx = minX.lane[lane];
int32 maxx = maxX.lane[lane];
int32 miny = minY.lane[lane];
int32 maxy = maxY.lane[lane];
VecS32 col = VecS32(minx) + colOffset;
VecS32 row = VecS32(miny) + rowOffset;
auto rowIdx = miny*size_.x + 2 * minx;
VecS32 w0_row = a0 * col + b0 * row + VecS32(C0.lane[lane]);
VecS32 w1_row = a1 * col + b1 * row + VecS32(C1.lane[lane]);
VecS32 w2_row = a2 * col + b2 * row + VecS32(C2.lane[lane]);
//Multiply each weight by two(rasterize 2x2 quad at once).
a0 = shiftl<1>(a0);
a1 = shiftl<1>(a1);
a2 = shiftl<1>(a2);
b0 = shiftl<1>(b0);
b1 = shiftl<1>(b1);
b2 = shiftl<1>(b2);
VecF32 zInc = itof(a1)*zz[1] + itof(a2)*zz[2];
for(int32 y = miny;y<=maxy;y+=2,rowIdx += 2 * size_.x){
auto w0 = w0_row;
auto w1 = w1_row;
auto w2 = w2_row;
VecF32 depth = zz[0] + itof(w1)*zz[1] + itof(w2)*zz[2];
auto idx = rowIdx;
for(int32 x = minx;x<=maxx;x+=2,idx+=4){
auto mask = w0|w1|w2;
VecF32 previousDepth = VecF32::load(data_+idx);
VecF32 mergedDepth = vmin(depth,previousDepth);
previousDepth = select(mergedDepth,previousDepth,mask);
previousDepth.store(data_+idx);
w0+=a0;
w1+=a1;
w2+=a2;
depth+=zInc;
}
w0_row += b0;
w1_row += b1;
w2_row += b2;
}
}
}
#endif
}
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);
}
}
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);
}
/*
* 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);
}
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++;
}
}
}
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;
}
}
}
}
}
}
}
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);
}
}
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;
}
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;
}
