Polyhedron Frustum::ToPolyhedron() const
{
// Note to maintainer: This function is an exact copy of AABB:ToPolyhedron() and OBB::ToPolyhedron().
Polyhedron p;
// Populate the corners of this Frustum.
// The will be in the order 0: ---, 1: --+, 2: -+-, 3: -++, 4: +--, 5: +-+, 6: ++-, 7: +++.
for(int i = 0; i < 8; ++i)
p.v.push_back(CornerPoint(i));
// Generate the 6 faces of this Frustum.
const int faces[6][4] =
{
{ 0, 2, 3, 1 }, // X-
{ 4, 5, 7, 6 }, // X+
{ 0, 1, 5, 4 }, // Y-
{ 7, 3, 2, 6 }, // Y+
{ 0, 4, 6, 2 }, // Z-
{ 1, 3, 7, 5 }, // Z+
};
for(int f = 0; f < 6; ++f)
{
Polyhedron::Face face;
for(int v = 0; v < 4; ++v)
face.v.push_back(faces[f][v]);
p.f.push_back(face);
}
return p;
}
AABB Frustum::MinimalEnclosingAABB() const
{
AABB aabb;
aabb.SetNegativeInfinity();
for(int i = 0; i < 8; ++i)
aabb.Enclose(CornerPoint(i));
return aabb;
}
void AABB::GetCornerPoints(vec *outPointArray) const
{
assume(outPointArray);
#ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
if (!outPointArray)
return;
#endif
for(int i = 0; i < 8; ++i)
outPointArray[i] = CornerPoint(i);
}
float3 Frustum::ExtremePoint(const float3 &direction) const
{
float3 mostExtreme = float3::nan;
float mostExtremeDist = -FLT_MAX;
for(int i = 0; i < 8; ++i)
{
float3 pt = CornerPoint(i);
float d = Dot(direction, pt);
if (d > mostExtremeDist)
{
mostExtremeDist = d;
mostExtreme = pt;
}
}
return mostExtreme;
}
std::vector<CornerPoint> CornerDetector::run(const std::vector<FieldLine> &lines) const
{
std::vector<FieldLine>::const_iterator it1, it2;
std::vector<CornerPoint> results;
if(lines.size() < 2) {
return results;
}
if(m_tolerance < 0 || m_tolerance > 1) {
errorlog << "CornerDetector::run called with invalid tolerance: " << m_tolerance << " (must be in [0, 1]." << std::endl;
return results;
}
for(it1 = lines.begin(); it1 != lines.end()-1; it1++) {
Line l1 = it1->getGroundLineEquation();
Vector2<NUPoint> l1_pts = it1->getEndPoints();
for(it2 = it1+1; it2 < lines.end(); it2++) {
Line l2 = it2->getGroundLineEquation();
Vector2<NUPoint> l2_pts = it2->getEndPoints();
if(l1.getAngleBetween(l2) > (1-m_tolerance)*mathGeneral::PI*0.5) {
//nearly perpendicular
//now build corner from end points
NUPoint intersection;
if(l1.getIntersection(l2, intersection.groundCartesian)) {
CornerPoint::TYPE type = findCorner(l1_pts, l2_pts, intersection, m_tolerance);
if(type != CornerPoint::INVALID) {
//need screen loc
if(it1->getScreenLineEquation().getIntersection(it2->getScreenLineEquation(), intersection.screenCartesian)) {
results.push_back(CornerPoint(type, intersection));
}
else {
errorlog << "CornerDetector::run - no intersection found for screen lines - transforms are probably not valid, not publishing corner" <<
"\t" << it1->getScreenLineEquation() << "\t" << it2->getScreenLineEquation() << std::endl;
}
}
}
}
}
}
return results;
}
Polyhedron Frustum::ToPolyhedron() const
{
// Note to maintainer: This function is an exact copy of AABB:ToPolyhedron() and OBB::ToPolyhedron().
Polyhedron p;
// Populate the corners of this Frustum.
// The will be in the order 0: ---, 1: --+, 2: -+-, 3: -++, 4: +--, 5: +-+, 6: ++-, 7: +++.
for(int i = 0; i < 8; ++i)
p.v.push_back(CornerPoint(i));
// Generate the 6 faces of this Frustum. The function Frustum::GetPlane() has a convention of returning
// the planes in order near,far,left,right,top,bottom, so follow the same convention here.
const int faces[6][4] =
{
{ 0, 4, 6, 2 }, // Z-: near plane
{ 1, 3, 7, 5 }, // Z+: far plane
{ 0, 2, 3, 1 }, // X-: left plane
{ 4, 5, 7, 6 }, // X+: right plane
{ 7, 3, 2, 6 }, // Y+: top plane
{ 0, 1, 5, 4 }, // Y-: bottom plane
};
for(int f = 0; f < 6; ++f)
{
Polyhedron::Face face;
if (this->handedness == FrustumLeftHanded)
{
for(int v = 0; v < 4; ++v)
face.v.push_back(faces[f][3-v]);
}
else
{
for(int v = 0; v < 4; ++v)
face.v.push_back(faces[f][v]);
}
p.f.push_back(face);
}
return p;
}
OBB Frustum::MinimalEnclosingOBB(float expandGuardband) const
{
assume(IsFinite());
assume(front.IsNormalized());
assume(up.IsNormalized());
OBB obb;
obb.pos = pos + (nearPlaneDistance + farPlaneDistance) * 0.5f * front;
obb.axis[1] = up;
obb.axis[2] = -front;
obb.axis[0] = Cross(obb.axis[1], obb.axis[2]);
obb.r = float3::zero;
for(int i = 0; i < 8; ++i)
obb.Enclose(CornerPoint(i));
// Expand the generated OBB very slightly to avoid numerical issues when
// testing whether this Frustum actually is contained inside the generated OBB.
obb.r.x += expandGuardband;
obb.r.y += expandGuardband;
obb.r.z += expandGuardband;
return obb;
}
Polyhedron AABB::ToPolyhedron() const
{
// Note to maintainer: This function is an exact copy of OBB:ToPolyhedron() and Frustum::ToPolyhedron().
Polyhedron p;
// Populate the corners of this AABB.
// The will be in the order 0: ---, 1: --+, 2: -+-, 3: -++, 4: +--, 5: +-+, 6: ++-, 7: +++.
for(int i = 0; i < 8; ++i)
p.v.push_back(CornerPoint(i));
// Generate the 6 faces of this AABB.
const int faces[6][4] =
{
{ 0, 1, 3, 2 }, // X-
{ 4, 6, 7, 5 }, // X+
{ 0, 4, 5, 1 }, // Y-
{ 7, 6, 2, 3 }, // Y+
{ 0, 2, 6, 4 }, // Z-
{ 1, 5, 7, 3 }, // Z+
};
for(int f = 0; f < 6; ++f)
{
Polyhedron::Face face;
for(int v = 0; v < 4; ++v)
face.v.push_back(faces[f][v]);
p.f.push_back(face);
}
assume(p.IsClosed());
assume(p.IsConvex());
assume(p.EulerFormulaHolds());
assume(p.FaceIndicesValid());
assume(p.FacesAreNondegeneratePlanar());
assume(p.Contains(this->CenterPoint()));
return p;
}
vec AABB::RandomCornerPoint(LCG &rng) const
{
return CornerPoint(rng.Int(0, 7));
}
void OBB::GetCornerPoints(float3 *outPointArray) const
{
assert(outPointArray);
for(int i = 0; i < 8; ++i)
outPointArray[i] = CornerPoint(i);
}
LineSegment OBB::Edge(int edgeIndex) const
{
assume(0 <= edgeIndex && edgeIndex <= 11);
switch(edgeIndex)
{
default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
case 0: return LineSegment(CornerPoint(0), CornerPoint(1));
case 1: return LineSegment(CornerPoint(0), CornerPoint(2));
case 2: return LineSegment(CornerPoint(0), CornerPoint(4));
case 3: return LineSegment(CornerPoint(1), CornerPoint(3));
case 4: return LineSegment(CornerPoint(1), CornerPoint(5));
case 5: return LineSegment(CornerPoint(2), CornerPoint(3));
case 6: return LineSegment(CornerPoint(2), CornerPoint(6));
case 7: return LineSegment(CornerPoint(3), CornerPoint(7));
case 8: return LineSegment(CornerPoint(4), CornerPoint(5));
case 9: return LineSegment(CornerPoint(4), CornerPoint(6));
case 10: return LineSegment(CornerPoint(5), CornerPoint(7));
case 11: return LineSegment(CornerPoint(6), CornerPoint(7));
}
}
ARFloat
Tracker::arMultiGetTransMatHull(ARMarkerInfo *marker_info, int marker_num, ARMultiMarkerInfoT *config)
{
//return arMultiGetTransMat(marker_info, marker_num, config);
int numInPoints=0;
std::vector<int> trackedMarkers;
int trackedCenterX=-1,trackedCenterY=-1;
//const int indices[4] = {idx0,idx1,idx2,idx3};
//rpp_vec ppos2d[4];
//rpp_vec ppos3d[4];
const int maxHullPoints = 16;
int indices[maxHullPoints];
rpp_vec ppos2d[maxHullPoints];
rpp_vec ppos3d[maxHullPoints];
// create an array of 2D points and keep references
// to the source points
//
for(int i=0; i<marker_num; i++)
{
int mId = marker_info[i].id;
int configIdx = -1;
for(int j=0; j<config->marker_num; j++)
if(config->marker[j].patt_id==mId)
{
configIdx = j;
break;
}
if(configIdx==-1)
continue;
trackedMarkers.push_back(i);
for(int c=0; c<4; c++)
{
int dir = marker_info[i].dir;
int cornerIdx = (c+4-dir)%4;
hullInPoints[numInPoints].x = (MarkerPoint::coord_type)marker_info[i].vertex[cornerIdx][0];
hullInPoints[numInPoints].y = (MarkerPoint::coord_type)marker_info[i].vertex[cornerIdx][1];
hullInPoints[numInPoints].cornerIdx = c;
hullInPoints[numInPoints].markerIdx = configIdx;
numInPoints++;
}
if(numInPoints>=MAX_HULL_POINTS)
break;
}
// next get the convex hull of all points
// (decrease amount by one to ignore last point, which is identical to first point)
//
int numHullPoints = nearHull_2D(hullInPoints, numInPoints, numInPoints, hullOutPoints)-1;
int idx0,idx1,idx2,idx3;
if(hullTrackingMode==HULL_FOUR && numHullPoints != 0)
{
// find those points with furthest distance and that lie on
// opposite parts of the hull. this fixes the first two points of our quad.
//
findLongestDiameter(hullOutPoints, numHullPoints, idx0,idx1);
assert(iabs(idx0-idx1)>0);
// find the point that is furthest away of the line
// of our first two points. this fixes the third point of the quad
findFurthestAway(hullOutPoints, numHullPoints, idx0,idx1, idx2);
sortIntegers(idx0,idx1, idx2);
// of all other points find the one that results in
// a quad with the largest area.
maximizeArea(hullOutPoints, numHullPoints, idx0,idx1,idx2,idx3);
// now that we have all four points we must sort them...
//
sortInLastInteger(idx0,idx1,idx2, idx3);
numHullPoints = 4;
indices[0] = idx0;
indices[1] = idx1;
indices[2] = idx2;
indices[3] = idx3;
}
else
{
if(numHullPoints>maxHullPoints)
numHullPoints = maxHullPoints;
for(int i=0; i<numHullPoints; i++)
indices[i] = i;
}
assert(numHullPoints<=maxHullPoints);
// create arrays of vertices for the 2D and 3D positions
//
//const int indices[4] = {idx0,idx1,idx2,idx3};
//rpp_vec ppos2d[4];
//rpp_vec ppos3d[4];
for(int i=0; i<numHullPoints; i++)
{
//int idx = indices[(i+1)%4];
int idx = indices[i];
const MarkerPoint& pt = hullOutPoints[idx];
trackedCorners.push_back(CornerPoint(pt.x,pt.y));
trackedCenterX += pt.x;
trackedCenterY += pt.y;
ppos2d[i][0] = pt.x;
ppos2d[i][1] = pt.y;
ppos2d[i][2] = 1.0f;
assert(pt.markerIdx < config->marker_num);
const ARMultiEachMarkerInfoT& markerInfo = config->marker[pt.markerIdx];
int cornerIdx = pt.cornerIdx;
ppos3d[i][0] = markerInfo.pos3d[cornerIdx][0];
ppos3d[i][1] = markerInfo.pos3d[cornerIdx][1];
ppos3d[i][2] = 0;
}
trackedCenterX /= 4;
trackedCenterY /= 4;
// prepare structures and data we need for input and output
// parameters of the rpp functions
const rpp_float cc[2] = {arCamera->mat[0][2],arCamera->mat[1][2]};
const rpp_float fc[2] = {arCamera->mat[0][0],arCamera->mat[1][1]};
rpp_float err = 1e+20;
rpp_mat R, R_init;
rpp_vec t;
if(poseEstimator==POSE_ESTIMATOR_RPP)
{
robustPlanarPose(err,R,t,cc,fc,ppos3d,ppos2d,numHullPoints,R_init, true,0,0,0);
if(err>1e+10)
return(-1); // an actual error has occurred in robustPlanarPose()
for(int k=0; k<3; k++)
{
config->trans[k][3] = (ARFloat)t[k];
for(int j=0; j<3; j++)
config->trans[k][j] = (ARFloat)R[k][j];
}
}
else
{
ARFloat rot[3][3];
int minIdx=-1, minDist=0x7fffffff;
for(size_t i=0; i<trackedMarkers.size(); i++)
{
assert(trackedMarkers[i]>=0 && trackedMarkers[i]<marker_num);
int idx = trackedMarkers[i];
const ARMarkerInfo& mInfo = marker_info[idx];
int dx = trackedCenterX-(int)mInfo.pos[0], dy = trackedCenterY-(int)mInfo.pos[1];
int d = dx*dx + dy*dy;
if(d<minDist)
{
minDist = d;
minIdx = (int)idx;
}
}
//trackedCorners.push_back(CornerPoint((int)marker_info[minIdx].pos[0],(int)marker_info[minIdx].pos[1]));
if(minIdx >= 0) {
return -1;
}
if(arGetInitRot(marker_info+minIdx, arCamera->mat, rot )<0)
return -1;
// finally use the normal pose estimator to get the pose
//
ARFloat tmp_pos2d[maxHullPoints][2], tmp_pos3d[maxHullPoints][2];
for(int i=0; i<numHullPoints; i++)
{
tmp_pos2d[i][0] = (ARFloat)ppos2d[i][0];
tmp_pos2d[i][1] = (ARFloat)ppos2d[i][1];
tmp_pos3d[i][0] = (ARFloat)ppos3d[i][0];
tmp_pos3d[i][1] = (ARFloat)ppos3d[i][1];
}
for(int i=0; i<AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ )
{
err = arGetTransMat3(rot, tmp_pos2d, tmp_pos3d, numHullPoints, config->trans, arCamera);
if(err<AR_GET_TRANS_MAT_MAX_FIT_ERROR)
break;
}
}
return (ARFloat)err;
}