void
IntersectActor::setupChildrenPriorities(void)
{
UInt32 numChildren = getNumChildren ();
UInt32 numExtraChildren = getNumExtraChildren();
Real32 scaleFactor = getScaleFactor();
Real32 hitDist = getHitDistance();
Real32 bvEnter;
Real32 bvExit;
setChildrenListEnabled(true);
for(UInt32 i = 0; i < numChildren; ++i)
{
#ifndef OSG_2_PREP
const DynamicVolume &vol = getChild(i)->editVolume(true);
#else
const BoxVolume &vol = getChild(i)->editVolume(true);
#endif
if((vol.intersect(getRay(), bvEnter, bvExit) == true ) &&
(bvEnter * scaleFactor < hitDist) )
{
setChildPriority(i, -bvEnter * scaleFactor);
}
else
{
setChildActive (i, false);
}
}
for(UInt32 i = 0; i < numExtraChildren; ++i)
{
#ifndef OSG_2_PREP
const DynamicVolume &vol = getChild(i)->editVolume(true);
#else
const BoxVolume &vol = getChild(i)->editVolume(true);
#endif
if((vol.intersect(getRay(), bvEnter, bvExit) == true ) &&
(bvEnter * scaleFactor < hitDist) )
{
setExtraChildPriority(i, -bvEnter * scaleFactor);
}
else
{
setExtraChildActive (i, false);
}
}
}
/**
* Performs a raytrace on the given pixel on the current scene.
* The pixel is relative to the bottom-left corner of the image.
* @param scene The scene to trace.
* @param x The x-coordinate of the pixel to trace.
* @param y The y-coordinate of the pixel to trace.
* @param width The width of the screen in pixels.
* @param height The height of the screen in pixels.
* @return The color of that pixel in the final image.
*/
static Color3 trace_pixel( const Scene* scene, size_t x, size_t y, size_t width, size_t height )
{
ray_t tRay;
real_t time;
real_t bestTime = 100000;
ray_t bestRay;
int bestGeom;
assert( 0 <= x && x < width );
assert( 0 <= y && y < height );
ray_t curRay = getRay(scene, x, y, width, height);
Geometry* const* geometries = scene->get_geometries();
Material* const* materials = scene->get_materials();
for ( size_t i = 0; i < scene->num_geometries(); i++ ) {
Geometry& geom = *geometries[i];
tRay = transform(curRay, geom);
time = geom.intersect(tRay);
real_t lastTime = time;
time = time / lastLength;
if( (time > 0) && (time < bestTime) ) {
bestTime = time;
bestGeom = i;
}
}
if(bestTime < 100000)
return calcColor(bestTime, curRay, bestGeom, scene, MAX_DEPTH);
else
return scene->background_color;
}
void RayModel::applyTranslation(double dx, double dy, double dz)
{
Vector3 d(dx,dy,dz);
for (int i = 0; i < getNbRay(); i++)
{
Ray ray = getRay(i);
ray.setOrigin(ray.origin() + d);
}
}
//-----------------------------------------------------------------------
void DefaultExtRaySceneQuery::execute( ExtRaySceneQueryListener* _listener )
{
mOgreQuery->setQueryMask( getQueryMask() );
mOgreQuery->setQueryTypeMask( getQueryTypeMask() );
mOgreQuery->setWorldFragmentType( getWorldFragmentType() );
mOgreQuery->setRay( getRay() );
BridgeQueryListener bql( _listener );
mOgreQuery->execute( &bql );
}
Frustum Camera::getVolume( float screenLeft, float screenRight, float screenTop, float screenBottom )
{
const Transform* transform = getEntity()->getTransform().get();
const Vector3& pos = transform->getPosition();
Frustum volume;
if(frustum.projection == FrustumProjection::Perspective)
{
Ray ul = getRay(screenLeft, screenTop);
Ray ur = getRay(screenRight, screenTop);
Ray bl = getRay(screenLeft, screenBottom);
Ray br = getRay(screenRight, screenBottom);
Vector3 normal;
// Planes order: Left, Right, Top, Bottom, Near, Far.
// Left plane
normal = bl.direction.cross(ul.direction);
volume.planes[0] = Plane(normal, pos);
// Right plane
normal = ur.direction.cross(br.direction);
volume.planes[1] = Plane(normal, pos);
// Top plane
normal = ul.direction.cross(ur.direction);
volume.planes[2] = Plane(normal, pos);
// Bottom plane
normal = br.direction.cross(bl.direction);
volume.planes[3] = Plane(normal, pos);
}
else
{
// Ortho planes are parallel to frustum planes
Ray ul = getRay(screenLeft, screenTop);
Ray br = getRay(screenRight, screenBottom);
volume.planes[0] = Plane(frustum.planes[0].normal, ul.origin);
volume.planes[1] = Plane(frustum.planes[1].normal, br.origin);
volume.planes[2] = Plane(frustum.planes[2].normal, ul.origin);
volume.planes[3] = Plane(frustum.planes[3].normal, br.origin);
}
// Near & Far plane applicable to both projection types
volume.planes[4] = frustum.planes[4];
volume.planes[5] = frustum.planes[5];
return volume;
}
int Camera::getLine(int mx, int my, const std::vector<LineSegment>& lines,
LineSegment& line) {
Ray ray = getRay(mx, my);
Vector3 v1 = ray.GetOrigin();
Vector3 v2 = v1 + ray.GetDirection() * 100.0f;
LineSegment l;
l.points[0] = v1;
l.points[1] = v2;
float minDist = 1000.0f;
int linePos = -1;
for (int i = 0; i < lines.size(); ++i) {
float dist = LineSegment::distSegmentSegment(lines[i], l);
if (dist <= 0.1f) {
minDist = dist;
line = lines[i];
linePos = i;
}
}
return linePos;
}
Vector<double> AncillaryMethods::backprojectGP(const Vector<double> &pos2D, const Camera &cam, const Vector<double> &gp)
{
Vector<double> gpN(3);
gpN(0) = (gp(0));
gpN(1) = (gp(1));
gpN(2) = (gp(2));
Vector<double> ray1;
Vector<double> ray2;
getRay(pos2D, ray1, ray2, cam);
Vector<double> point3D;
intersectPlane(gpN, gp(3), ray1, ray2, point3D);
point3D *= Globals::WORLD_SCALE;
return point3D;
}
bool Camera::getPoint(int mx, int my, const std::vector<LineSegment>& lines,
Vector3& p, const Plane& plane) {
float minDist = 1000.0f;
bool findCurr = false;
Ray ray = getRay(mx, my);
for (int i = 0; i < lines.size(); ++i) {
for (int j = 0; j < 2; ++j) {
if (Ray::distRayPoint(ray, lines[i].points[j]) < 0.1f) {
if ((ray.GetOrigin() - lines[i].points[j]).length() < minDist) {
minDist = (ray.GetOrigin() - lines[i].points[j]).length();
p = lines[i].points[j];
findCurr = true;
}
}
}
}
if (!findCurr)
p = intersect(ray, plane);
// std::cout<<p<<std::endl;
return findCurr;
}
// x,y: pixel position on the screen
void rayCasting::castRay(int x, int y){
glm::vec4 rayDir = getRay(x,y);
glm::vec4 destColor = glm::vec4(0.0);
float tmin, tmax;
if (intersect(rayDir,tmin, tmax) == true){
glm::vec4 pos = eye + tmin*rayDir;
int index[3], indices[8], indexLow[3], indexHigh[3];
float offset[3], distToCellCenter[3];
getVolumePosition(index,offset, pos);
indexLow[0] = index[0]; indexLow[1] = index[1]; indexLow[2] = index[2];
indexHigh[0] = index[0]; indexHigh[1] = index[1]; indexHigh[2] = index[2];
if (pos.x < 0.5){
indexLow[0] = indexLow[0]-1;
distToCellCenter[0] = 0.5 + pos.x;
}else{
indexHigh[0] = indexHigh[0]+1;
distToCellCenter[0] = pos.x - 0.5;
}
if (pos.y < 0.5){
indexLow[1] = indexLow[1]-1;
distToCellCenter[1] = 0.5 + pos.y;
}else{
indexHigh[1] = indexHigh[1]+1;
distToCellCenter[1] = pos.y - 0.5;
}
if (pos.z < 0.5){
indexLow[2] = indexLow[2]-1;
distToCellCenter[1] = 0.5 + pos.z;
}else{
indexHigh[2] = indexHigh[2]+1;
distToCellCenter[1] = pos.z - 0.5;
}
// Compute indices
indices[0] = computeIndex(logicalBounds,indexLow[0] ,indexLow[1] ,indexLow[2]);
indices[1] = computeIndex(logicalBounds,indexHigh[0],indexLow[1] ,indexLow[2]);
indices[2] = computeIndex(logicalBounds,indexLow[0] ,indexHigh[1],indexLow[2]);
indices[3] = computeIndex(logicalBounds,indexHigh[0],indexHigh[1],indexLow[2]);
indices[4] = computeIndex(logicalBounds,indexLow[0] ,indexLow[1] ,indexHigh[2]);
indices[5] = computeIndex(logicalBounds,indexHigh[0],indexLow[1] ,indexHigh[2]);
indices[6] = computeIndex(logicalBounds,indexLow[0] ,indexHigh[1],indexHigh[2]);
indices[7] = computeIndex(logicalBounds,indexHigh[0],indexHigh[1],indexHigh[2]);
// Get the scalar values of the 8 surrounding cells
float scalarValues[8];
assignEight(scalarValues, indices);
float scalar = trilinearInterpolate(scalarValues, 1.0-distToCellCenter[0], 1.0-distToCellCenter[1], 1.0-distToCellCenter[2]);
// Get the color for that scalar value
glm::vec4 srcColor = glm::vec4(0.0);
QueryTF(scalar, srcColor);
// lighting
glm::vec3 gradient = glm::vec3(0.0);
// compute gradient
destColor = colorScalar(gradient, rayDir.xyz(), srcColor, destColor);
}
}
double Camera::bbToDetection(const Vector<double>& bbox, Vector<double>& pos3D, double ConvertScale, double& dist)
{
// pos3D.clearContent();
pos3D.setSize(3);
double x = bbox(0);
double y = bbox(1);
double w = bbox(2);
double h = bbox(3);
// bottom_left and bottom_right are the point of the BBOX
Vector<double> bottom_left(3, 1.0);
bottom_left(0) = x + w/2.0;
bottom_left(1) = y + h;
Vector<double> bottom_right(3, 1.0);
bottom_right(0) = x + w;
bottom_right(1) = y + h;
Vector<double> ray_bot_left_1;
Vector<double> ray_bot_left_2;
Vector<double> ray_bot_right_1;
Vector<double> ray_bot_right_2;
// Backproject through base point
getRay(bottom_left, ray_bot_left_1, ray_bot_left_2);
getRay(bottom_right, ray_bot_right_1, ray_bot_right_2);
Vector<double> gpPointLeft;
Vector<double> gpPointRight;
// Intersect with ground plane
intersectPlane(GPN_, GPD_, ray_bot_left_1, ray_bot_left_2, gpPointLeft);
intersectPlane(GPN_, GPD_, ray_bot_right_1, ray_bot_right_2, gpPointRight);
// Find top point
Vector<double> ray_top_1;
Vector<double> ray_top_2;
Vector<double> aux(3, 1.0);
aux(0) = x;
aux(1) = y;
getRay(aux, ray_top_1, ray_top_2);
// Vertical plane through base points + normal
Vector<double> point3;
point3 = gpPointLeft;
point3 -= (GPN_);
Vector<double> vpn(3,0.0);
Vector<double> diffGpo1Point3;
Vector<double> diffGpo2Point3;
diffGpo1Point3 = gpPointLeft;
diffGpo1Point3 -=(point3);
diffGpo2Point3 = gpPointRight;
diffGpo2Point3 -= point3;
vpn = cross(diffGpo1Point3,diffGpo2Point3);
double vpd = (-1.0)*DotProduct(vpn, point3);
Vector<double> gpPointTop;
intersectPlane(vpn, vpd, ray_top_1, ray_top_2, gpPointTop);
// Results
gpPointTop -= gpPointLeft;
// Compute Size
double dSize = gpPointTop.norm();
// Compute Distance
aux = t_;
aux -= gpPointLeft;
dist = aux.norm();
dist = dist * ConvertScale;
if(gpPointLeft(2) < t_(2)) dist = dist * (-1.0);
// Compute 3D Position of BBOx
double posX = gpPointLeft(0) * ConvertScale;
double posY = gpPointLeft(1) * ConvertScale;
double posZ = gpPointLeft(2) * ConvertScale;
pos3D(0) = (posX);
pos3D(1) = (posY);
pos3D(2) = (posZ);
return dSize * ConvertScale;
}
Ray Screen::getRay(unsigned int image_width, unsigned int x, unsigned int y) const {
return getRay( double(x) / double(image_width), double(y) / double(getImageHeight(image_width)));
}
bool DepthBuffer::testAABB(vec3f min,vec3f max){
vec4f vertices[8];
gatherBoxVertices(vertices,min,max);
transformBoxVertices(vertices,viewProjection_);
boxVerticesToScreenVertices(vertices,clipSpaceToScreenSpaceMultiplier,center_);
ScreenSpaceQuad faces[6];
auto faceCount = extractBoxQuads(faces,vertices,aabbFrontFaceMask);
for(uint32 i = 0;i<faceCount;++i){
if(testTriangle2x2(faces[i].v[0],faces[i].v[1],faces[i].v[2])) return true;
if(testTriangle2x2(faces[i].v[2],faces[i].v[3],faces[i].v[0])) return true;
}
return false;
//Transform the AABB vertices to Homogenous clip space
//vec4f vertices[8];
vertices[0] = vec4f(min.x,min.y,min.z,1);
vertices[1] = vec4f(max.x,min.y,min.z,1);
vertices[2] = vec4f(max.x,max.y,min.z,1);
vertices[3] = vec4f(min.x,max.y,min.z,1);
vertices[4] = vec4f(min.x,min.y,max.z,1);
vertices[5] = vec4f(max.x,min.y,max.z,1);
vertices[6] = vec4f(max.x,max.y,max.z,1);
vertices[7] = vec4f(min.x,max.y,max.z,1);
vec4f clipMin,clipMax;
for(uint32 i =0;i<8;++i){
vertices[i] = viewProjection_ * vertices[i];
vertices[i] = vertices[i] * (1.0f/vertices[i].w);
//Homogenous coordinates => non-homogenous coordinates
//Determine the min/max for the screen aabb
clipMin = vec4f::min(vertices[i],clipMin);
clipMax = vec4f::max(vertices[i],clipMax);
}
//Determine the screen aabb which covers the box
//Clip space coordinates => screen coordinates [-1,1] -> [-320,320] -> [0,640]
clipMin = vec4f::max(clipMin,vec4f(-1.0f,-1.0f,-1.0f,-1.0f))*clipSpaceToScreenSpaceMultiplier;
clipMax = vec4f::min(clipMax,vec4f(1.0f,1.0f,1.0f,1.0f))*clipSpaceToScreenSpaceMultiplier;
vec2i screenMin = vec2i(int32(clipMin.x),int32(clipMin.y))+center_;
screenMin.x &= (~1);screenMin.y &= (~1);
vec2i screenMax = vec2i(int32(clipMax.x),int32(clipMax.y))+center_;
auto rowIdx = screenMin.y*size_.x + 2 * screenMin.x;
float minZ = vertices[0].z;
for(uint32 i = 1;i< 8;i++){
minZ = std::min(minZ,vertices[i].z);
}
VecF32 flatDepth(minZ);
//Iterate over the pixels
for(;screenMin.y < screenMax.y;screenMin.y+=2,rowIdx += 2 * size_.x){
auto idx = rowIdx;
for(int32 x = screenMin.x;x<screenMax.x;x+=2,idx+=4){
//Fetch the distance value for the current pixel.
auto depth = VecF32::load(data_ + idx);
vec3f rayo,rayd;
getRay(rayo,rayd,x,screenMin.y);
float dist;
if(!rayAABBIntersect(min,max,rayo,rayd,dist)) continue;
dist = ((dist-znear_)/zfar_ ) * 2.0f - 1.0f;
VecF32 flatDepth(dist);
flatDepth.store(data_+idx);
auto mask = _mm_movemask_ps(cmple(flatDepth,depth).simd);
//if(mask != 0)//{
//return true; //Visible
//}
//
//
//Compute the distance to the aabb (raytrace)
//vec3f rayo,rayd;
//getRay(rayo,rayd,x,screenMin.y);
//float dist;
//Convert the distance from view space to depth space [-1,1]
//dist = ((dist-znear_)/zfar_ ) * 2.0f - 1.0f;
//Compare the values.
//if(dist <= depth){
//return true;
// data_[idx] = dist;
//}
} }
return false;
}
cv::Mat SimulatingImage::projectPlane(int pattern) {
cv::Mat img = cv::Mat::zeros(img_size.height, img_size.width, CV_8UC1);
calcCorners();
for (int y = 0; y < img_size.height; ++y) {
for (int x = 0; x < img_size.width; ++x) {
cv::Point3d ray = getRay(cv::Point2d(x,y));
if (isCross(ray)) {
cv::Point3d p = calcCrossPoint(ray);
double s = calcS(p);
double t = calcT(p);
if (0 <= s && s <= 1 && 0 <= t && t <= 1) {
double step;
switch (pattern) {
case 0: // check
step = interval / pattern_size.width;
for (int i = 0; 2*i*step <= 1.0; ++i) {
if (step*(2*i) <= s && s <= step*(2*i+1)) {
img.at<uchar>(y,x) = 255;
break;
}
}
step = interval / pattern_size.height;
for (int i = 0; 2*i*step <= 1.0; ++i) {
if (step*(2*i) <= t && t <= step*(2*i+1)) {
img.at<uchar>(y,x) = abs(img.at<uchar>(y,x)-255);
break;
}
}
break;
case 1: // vertical strip
step = interval / pattern_size.width;
for (int i = 0; 2*i*step <= 1.0; ++i) {
if (step*(2*i) <= s && s <= step*(2*i+1)) {
img.at<uchar>(y,x) = 255;
break;
}
}
break;
case 2: // inverse of 1
step = interval / pattern_size.width;
for (int i = 0; (2*i+1)*step <= 1.0; ++i) {
if (step*(2*i+1) <= s && s <= step*(2*i+2)) {
img.at<uchar>(y,x) = 255;
break;
}
}
break;
case 3: // horizontal strip
step = interval / pattern_size.height;
for (int i = 0; 2*i*step <= 1.0; ++i) {
if (step*(2*i) <= t && t <= step*(2*i+1)) {
img.at<uchar>(y,x) = 255;
break;
}
}
break;
case 4: // inverse of 3
step = interval / pattern_size.height;
for (int i = 0; (2*i+1)*step <= 1.0; ++i) {
if (step*(2*i+1) <= t && t <= step*(2*i+2)) {
img.at<uchar>(y,x) = 255;
break;
}
}
break;
}
}
}
}
}
return img;
}
