| 1 |
#include "PlanePrimitiveShape.h" |
| 2 |
#include "ScoreComputer.h" |
| 3 |
#include "PrimitiveShapeVisitor.h" |
| 4 |
#include <GfxTL/NullClass.h> |
| 5 |
#include <limits> |
| 6 |
#include <iostream> |
| 7 |
#include <fstream> |
| 8 |
#include <sstream> |
| 9 |
#include <iomanip> |
| 10 |
#include <MiscLib/Performance.h> |
| 11 |
extern MiscLib::performance_t totalTime_planeConnected; |
| 12 |
|
| 13 |
PlanePrimitiveShape::PlanePrimitiveShape(const Vec3f &a, const Vec3f &b, |
| 14 |
const Vec3f &c) |
| 15 |
: m_plane(a, b, c) |
| 16 |
{ |
| 17 |
m_hcs.FromNormal(m_plane.getNormal()); |
| 18 |
} |
| 19 |
|
| 20 |
PlanePrimitiveShape::PlanePrimitiveShape(const Plane &plane) |
| 21 |
: m_plane(plane) |
| 22 |
{ |
| 23 |
m_hcs.FromNormal(m_plane.getNormal()); |
| 24 |
} |
| 25 |
|
| 26 |
size_t PlanePrimitiveShape::Identifier() const |
| 27 |
{ |
| 28 |
return 0; |
| 29 |
} |
| 30 |
|
| 31 |
PrimitiveShape *PlanePrimitiveShape::Clone() const |
| 32 |
{ |
| 33 |
return new PlanePrimitiveShape(*this); |
| 34 |
} |
| 35 |
|
| 36 |
float PlanePrimitiveShape::Distance(const Vec3f &p) const |
| 37 |
{ |
| 38 |
return m_plane.getDistance(p); |
| 39 |
} |
| 40 |
|
| 41 |
float PlanePrimitiveShape::SignedDistance(const Vec3f &p) const |
| 42 |
{ |
| 43 |
return m_plane.SignedDistance(p); |
| 44 |
} |
| 45 |
|
| 46 |
float PlanePrimitiveShape::NormalDeviation(const Vec3f &, |
| 47 |
const Vec3f &n) const |
| 48 |
{ |
| 49 |
return m_plane.getNormal().dot(n); |
| 50 |
} |
| 51 |
|
| 52 |
void PlanePrimitiveShape::DistanceAndNormalDeviation( |
| 53 |
const Vec3f &p, const Vec3f &n, std::pair< float, float > *dn) const |
| 54 |
{ |
| 55 |
dn->first = m_plane.getDistance(p); |
| 56 |
dn->second = m_plane.getNormal().dot(n); |
| 57 |
} |
| 58 |
|
| 59 |
void PlanePrimitiveShape::Project(const Vec3f &p, Vec3f *pp) const |
| 60 |
{ |
| 61 |
*pp = p - ((m_plane.getNormal().dot(p - m_plane.getPosition())) |
| 62 |
* m_plane.getNormal()); |
| 63 |
} |
| 64 |
|
| 65 |
void PlanePrimitiveShape::Normal(const Vec3f &p, Vec3f *n) const |
| 66 |
{ |
| 67 |
*n = m_plane.getNormal(); |
| 68 |
} |
| 69 |
|
| 70 |
unsigned int PlanePrimitiveShape::ConfidenceTests(unsigned int numTests, |
| 71 |
float epsilon, float normalThresh, float rms, const PointCloud &pc, |
| 72 |
const MiscLib::Vector< size_t > &indices) const |
| 73 |
{ |
| 74 |
return BasePrimitiveShape::ConfidenceTests< Plane >(numTests, epsilon, |
| 75 |
normalThresh, rms, pc, indices); |
| 76 |
} |
| 77 |
|
| 78 |
void PlanePrimitiveShape::Description(std::string *s) const |
| 79 |
{ |
| 80 |
*s = "Plane"; |
| 81 |
} |
| 82 |
|
| 83 |
bool PlanePrimitiveShape::Fit(const PointCloud &pc, float epsilon, |
| 84 |
float normalThresh, MiscLib::Vector< size_t >::const_iterator begin, |
| 85 |
MiscLib::Vector< size_t >::const_iterator end) |
| 86 |
{ |
| 87 |
Plane fit = m_plane; |
| 88 |
//if(fit.LeastSquaresFit(pc, begin, end)) |
| 89 |
if(fit.LeastSquaresFit(pc, begin, end)) |
| 90 |
{ |
| 91 |
m_plane = fit; |
| 92 |
m_hcs.FromNormal(m_plane.getNormal()); |
| 93 |
return true; |
| 94 |
} |
| 95 |
return false; |
| 96 |
} |
| 97 |
|
| 98 |
PrimitiveShape *PlanePrimitiveShape::LSFit(const PointCloud &pc, float epsilon, |
| 99 |
float normalThresh, MiscLib::Vector< size_t >::const_iterator begin, |
| 100 |
MiscLib::Vector< size_t >::const_iterator end, |
| 101 |
std::pair< size_t, float > *score) const |
| 102 |
{ |
| 103 |
Plane fit = m_plane; |
| 104 |
if(fit.LeastSquaresFit(pc, begin, end)) |
| 105 |
{ |
| 106 |
score->first = -1; |
| 107 |
return new PlanePrimitiveShape(fit); |
| 108 |
} |
| 109 |
score->first = 0; |
| 110 |
return NULL; |
| 111 |
} |
| 112 |
|
| 113 |
void PlanePrimitiveShape::Serialize(std::ostream *o, bool binary) const |
| 114 |
{ |
| 115 |
if(binary) |
| 116 |
{ |
| 117 |
const char id = 0; |
| 118 |
(*o) << id; |
| 119 |
} |
| 120 |
else |
| 121 |
(*o) << "0" << " "; |
| 122 |
m_plane.Serialize(binary, o); |
| 123 |
if(!binary) |
| 124 |
*o << std::endl; |
| 125 |
} |
| 126 |
|
| 127 |
size_t PlanePrimitiveShape::SerializedSize() const |
| 128 |
{ |
| 129 |
return m_plane.SerializedSize() + 1; |
| 130 |
} |
| 131 |
|
| 132 |
void PlanePrimitiveShape::Transform(float scale, const Vec3f &translate) |
| 133 |
{ |
| 134 |
m_plane.Transform(scale, translate); |
| 135 |
} |
| 136 |
|
| 137 |
void PlanePrimitiveShape::Transform(const GfxTL::MatrixXX< 3, 3, float > &rot, |
| 138 |
const GfxTL::Vector3Df &trans) |
| 139 |
{ |
| 140 |
GfxTL::Vector3Df transNormal = rot * GfxTL::Vector3Df( |
| 141 |
m_plane.getNormal()); |
| 142 |
GfxTL::Vector3Df transOrigin = rot * GfxTL::Vector3Df( |
| 143 |
m_plane.getPosition()) + trans; |
| 144 |
m_plane = Plane(Vec3f(transOrigin.Data()), Vec3f(transNormal.Data())); |
| 145 |
m_hcs[0] = rot * m_hcs[0]; |
| 146 |
m_hcs[1] = rot * m_hcs[1]; |
| 147 |
} |
| 148 |
|
| 149 |
void PlanePrimitiveShape::Visit(PrimitiveShapeVisitor *visitor) const |
| 150 |
{ |
| 151 |
visitor->Visit(*this); |
| 152 |
} |
| 153 |
|
| 154 |
bool PlanePrimitiveShape::Similar(float, const PlanePrimitiveShape &) const |
| 155 |
{ |
| 156 |
return true; // planes are always similar! |
| 157 |
} |
| 158 |
|
| 159 |
LevMarFunc< float > *PlanePrimitiveShape::SignedDistanceFunc() const |
| 160 |
{ |
| 161 |
return new PlaneLevMarFunc(m_plane); |
| 162 |
} |
| 163 |
|
| 164 |
void PlanePrimitiveShape::Parameters(const Vec3f &p, |
| 165 |
std::pair< float, float > *param) const |
| 166 |
{ |
| 167 |
Vec3f pp = p - m_plane.getPosition(); |
| 168 |
param->first = pp.dot(m_hcs[0].Data()); |
| 169 |
param->second = pp.dot(m_hcs[1].Data()); |
| 170 |
} |
| 171 |
|
| 172 |
void PlanePrimitiveShape::Parameters( |
| 173 |
GfxTL::IndexedIterator< MiscLib::Vector< size_t >::iterator, |
| 174 |
PointCloud::const_iterator > begin, |
| 175 |
GfxTL::IndexedIterator< MiscLib::Vector< size_t >::iterator, |
| 176 |
PointCloud::const_iterator > end, |
| 177 |
MiscLib::Vector< std::pair< float, float > > *bmpParams) const |
| 178 |
{ |
| 179 |
ParametersImpl(begin, end, bmpParams); |
| 180 |
} |
| 181 |
|
| 182 |
void PlanePrimitiveShape::Parameters( |
| 183 |
GfxTL::IndexedIterator< IndexIterator, |
| 184 |
PointCloud::const_iterator > begin, |
| 185 |
GfxTL::IndexedIterator< IndexIterator, |
| 186 |
PointCloud::const_iterator > end, |
| 187 |
MiscLib::Vector< std::pair< float, float > > *bmpParams) const |
| 188 |
{ |
| 189 |
ParametersImpl(begin, end, bmpParams); |
| 190 |
} |
| 191 |
|
| 192 |
void PlanePrimitiveShape::BitmapExtent(float epsilon, |
| 193 |
GfxTL::AABox< GfxTL::Vector2Df > *bbox, |
| 194 |
MiscLib::Vector< std::pair< float, float > > *params, |
| 195 |
size_t *uextent, size_t *vextent) |
| 196 |
{ |
| 197 |
*uextent = size_t(std::ceil((bbox->Max()[0] - bbox->Min()[0]) / epsilon)) + 1; |
| 198 |
*vextent = size_t(std::ceil((bbox->Max()[1] - bbox->Min()[1]) / epsilon)) + 1; |
| 199 |
} |
| 200 |
|
| 201 |
void PlanePrimitiveShape::InBitmap(const std::pair< float, float > ¶m, |
| 202 |
float epsilon, const GfxTL::AABox< GfxTL::Vector2Df > &bbox, size_t, |
| 203 |
size_t, std::pair< int, int > *inBmp) const |
| 204 |
{ |
| 205 |
inBmp->first = (int)std::floor((param.first - bbox.Min()[0]) / epsilon); |
| 206 |
inBmp->second = (int)std::floor((param.second - bbox.Min()[1]) / epsilon); |
| 207 |
} |
| 208 |
void PlanePrimitiveShape::WrapBitmap( |
| 209 |
const GfxTL::AABox< GfxTL::Vector2Df > &bbox, float epsilon, bool *uwrap, |
| 210 |
bool *vwrap) const |
| 211 |
{ |
| 212 |
*uwrap = false; |
| 213 |
*vwrap = false; |
| 214 |
} |
| 215 |
|
| 216 |
void PlanePrimitiveShape::SetExtent( |
| 217 |
const GfxTL::AABox< GfxTL::Vector2Df > &bbox, const MiscLib::Vector< int > &, |
| 218 |
size_t, size_t, float, int) |
| 219 |
{} |
| 220 |
|
| 221 |
bool PlanePrimitiveShape::InSpace(float u, float v, Vec3f *p, Vec3f *n) const |
| 222 |
{ |
| 223 |
*p = m_plane.getPosition() + Vec3f((u * m_hcs[0] + v * m_hcs[1]).Data()); |
| 224 |
*n = m_plane.getNormal(); |
| 225 |
return true; |
| 226 |
} |
| 227 |
|
| 228 |
bool PlanePrimitiveShape::InSpace(size_t u, size_t v, float epsilon, |
| 229 |
const GfxTL::AABox< GfxTL::Vector2Df > &bbox, size_t uextent, |
| 230 |
size_t vextent, Vec3f *p, Vec3f *n) const |
| 231 |
{ |
| 232 |
*p = Vec3f(((bbox.Min()[0] + epsilon * (float(u) + .5f)) * m_hcs[0] + |
| 233 |
(bbox.Min()[1] + epsilon * (float(v) + .5f)) * m_hcs[1]).Data()) + |
| 234 |
m_plane.getPosition(); |
| 235 |
*n = m_plane.getNormal(); |
| 236 |
return true; |
| 237 |
} |