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; }