void ComputeAabbFromPoints(SAxisAlignedBox* aabb, u32 count, const XMFLOAT3 points[])
{
XMFLOAT3 maxPoint(-FLT_MAX, -FLT_MAX, -FLT_MAX);
XMFLOAT3 minPoint(FLT_MAX, FLT_MAX, FLT_MAX);
for (u32 i = 0; i < count; i++)
{
XMFLOAT3 point = points[i];
if (point.x > maxPoint.x)
maxPoint.x = point.x;
if (point.x < minPoint.x)
minPoint.x = point.x;
if (point.y > maxPoint.y)
maxPoint.y = point.y;
if (point.y < minPoint.y)
minPoint.y = point.y;
if (point.z > maxPoint.z)
maxPoint.z = point.z;
if (point.z < minPoint.z)
minPoint.z = point.z;
}
aabb->Center = XMFLOAT3((maxPoint.x + minPoint.x) * 0.5f,
(maxPoint.y + minPoint.y) * 0.5f,
(maxPoint.z + minPoint.z) * 0.5f);
aabb->Extents = XMFLOAT3((maxPoint.x - minPoint.x) * 0.5f,
(maxPoint.y - minPoint.y) * 0.5f,
(maxPoint.z - minPoint.z) * 0.5f);
}
/**
* Split the bounding box in several bounding boxes at limitSize.
* If original box doesn't exceed limit size, the original box is returned.
*/
std::vector<AABBox<float>> SplitBoundingBox::split(const AABBox<float> &aabbox) const
{
std::vector<float> axisSplits[3];
for(unsigned int axis=0; axis<3; ++axis)
{
float size = aabbox.getHalfSize(axis) * 2.0f;
axisSplits[axis].push_back(aabbox.getMin()[axis]);
auto nbSplits = static_cast<int>(std::ceil(size/limitSize));
float maxValue = aabbox.getMax()[axis];
for(int split = 0; split < nbSplits; ++split)
{
float splitValue = std::min(aabbox.getMin()[axis] + limitSize*(split+1), maxValue);
axisSplits[axis].push_back(splitValue);
}
}
std::vector<AABBox<float>> splittedBoundingBox;
for(unsigned int x=1; x<axisSplits[0].size(); ++x)
{
for(unsigned int y=1; y<axisSplits[1].size(); ++y)
{
for(unsigned int z=1; z<axisSplits[2].size(); ++z)
{
Point3<float> minPoint(axisSplits[0][x-1], axisSplits[1][y-1], axisSplits[2][z-1]);
Point3<float> maxPoint(axisSplits[0][x], axisSplits[1][y], axisSplits[2][z]);
splittedBoundingBox.emplace_back(AABBox<float>(minPoint, maxPoint));
}
}
}
return splittedBoundingBox;
}
void Font::drawGlyphs(GraphicsContext* context, const SimpleFontData* font, const GlyphBuffer& glyphBuffer, int from, int numGlyphs, const FloatPoint& point) const
{
FS_STATE* iType = font->platformData().scaledFont(context->getCTM().yScale());
if (!iType)
return;
FloatPoint adjustedPoint = point;
if (font->platformData().orientation() == Vertical)
adjustedPoint.move(-(font->fontMetrics().floatAscent(IdeographicBaseline) - font->fontMetrics().floatAscent()), 0);
bool softwareBlurRequired = context->state().shadowBlur / font->fontMetrics().xHeight() > 0.5;
FloatSize currentShadowOffset;
float currentShadowBlur;
Color currentShadowColor;
ColorSpace currentColorSpace;
// If we have a shadow blur, and it is too big to apply at text-render time, we must render it now.
if (context->hasShadow() && softwareBlurRequired) {
context->getShadow(currentShadowOffset, currentShadowBlur, currentShadowColor, currentColorSpace);
const GraphicsContextState state = context->state();
FloatSize offset = state.shadowOffset;
if (state.shadowsIgnoreTransforms)
offset.setHeight(-offset.height());
ShadowBlur shadow(FloatSize(state.shadowBlur, state.shadowBlur), offset, state.shadowColor, state.shadowColorSpace);
FloatPoint minPoint(adjustedPoint.x(), adjustedPoint.y() - fontMetrics().ascent());
FloatPoint maxPoint(adjustedPoint.x(), adjustedPoint.y() + fontMetrics().descent());
FloatPoint currentPoint = adjustedPoint;
for (int i = 0; i < numGlyphs; ++i) {
currentPoint += *glyphBuffer.advances(from + i);
minPoint.setX(std::min(minPoint.x(), currentPoint.x()));
minPoint.setY(std::min(minPoint.y(), currentPoint.y()));
maxPoint = maxPoint.expandedTo(currentPoint);
}
const FloatRect boundingRect(minPoint.x(), minPoint.y(), maxPoint.x() - minPoint.x(), maxPoint.y() - minPoint.y());
GraphicsContext* shadowContext = shadow.beginShadowLayer(context, boundingRect);
if (shadowContext) {
iType = font->platformData().scaledFont(shadowContext->getCTM().yScale());
shadowContext->platformContext()->addGlyphs(glyphBuffer.glyphs(from),
reinterpret_cast<const BlackBerry::Platform::FloatSize*>(glyphBuffer.advances(from)),
numGlyphs, adjustedPoint, iType, 0, 0);
iType = font->platformData().scaledFont(context->getCTM().yScale());
shadow.endShadowLayer(context);
}
context->platformContext()->clearShadow();
}
context->platformContext()->addGlyphs(glyphBuffer.glyphs(from),
reinterpret_cast<const BlackBerry::Platform::FloatSize*>(glyphBuffer.advances(from)),
numGlyphs, adjustedPoint, iType,
context->fillGradient() ? context->fillGradient()->platformGradient() : (context->fillPattern() ? context->fillPattern()->platformPattern(AffineTransform()) : static_cast<BlackBerry::Platform::Graphics::Paint*>(0)),
context->strokeGradient() ? context->strokeGradient()->platformGradient() : (context->strokePattern() ? context->strokePattern()->platformPattern(AffineTransform()) : static_cast<BlackBerry::Platform::Graphics::Paint*>(0)));
if (softwareBlurRequired)
context->platformContext()->setShadow(currentShadowOffset, currentShadowBlur, currentShadowColor.isValid() ? currentShadowColor.rgb() : makeRGBA(0, 0, 0, 0xFF / 3), context->state().shadowsIgnoreTransforms);
}
//获取最大点
PointCoord DcGp::DcGpPointCloud::GetMaxPoint() const
{
m_pDcGpPointCloudImpl->CalculateMyOwnBoundBox();
PointCoord maxPoint(m_pDcGpPointCloudImpl->m_maxPoint.x,
m_pDcGpPointCloudImpl->m_maxPoint.y,
m_pDcGpPointCloudImpl->m_maxPoint.z);
return maxPoint;
}
void Cone3D::buildTexCoords(IDirect3DDevice9* gd3dDevice)
{
D3DVERTEXELEMENT9 elements[64];
UINT numElements = 0;
VertexPos::Decl->GetDeclaration(elements, &numElements);
ID3DXMesh* temp = 0;
HR(mpMesh->CloneMesh(D3DXMESH_SYSTEMMEM,
elements, gd3dDevice, &temp));
ReleaseCOM(mpMesh);
// Now generate texture coordinates for each vertex.
VertexPos* vertices = 0;
HR(temp->LockVertexBuffer(0, (void**)&vertices));
D3DXVECTOR3 maxPoint(-FLT_MAX, -FLT_MAX, -FLT_MAX);
D3DXVECTOR3 minPoint(FLT_MAX, FLT_MAX, FLT_MAX);
for (UINT i = 0; i < temp->GetNumVertices(); ++i)
{
D3DXVec3Maximize(&maxPoint, &maxPoint, &vertices[i].mPos);
D3DXVec3Minimize(&minPoint, &minPoint, &vertices[i].mPos);
}
float a = minPoint.z;
float b = maxPoint.z;
float h = b - a;
for (int i = 0; i < (int)temp->GetNumVertices(); i++)
{
float x = vertices[i].mPos.x;
float y = vertices[i].mPos.z;
float z = vertices[i].mPos.y;
float theta = atan2f(z, x);
float y2 = y - b;
float u = theta / (2.0f * D3DX_PI);
float v = y2 / -h;
vertices[i].mTexCoord.x = u;
vertices[i].mTexCoord.y = v;
}
HR(temp->UnlockVertexBuffer());
HR(temp->CloneMesh(D3DXMESH_MANAGED | D3DXMESH_WRITEONLY,
elements, gd3dDevice, &mpMesh));
ReleaseCOM(temp);
}
/*!
\details
No detailed.
*/
inline
uint Aabb::longestAxis() const noexcept
{
constexpr uint x = 0;
constexpr uint y = 1;
constexpr uint z = 2;
const auto range = maxPoint() - minPoint();
return (range[y] < range[x])
? (range[z] < range[x])
? x
: z
: (range[z] < range[y])
? y
: z;
}
void ComputeAabbFromOrientedBox(SAxisAlignedBox* aabb, const SOrientedBox& obb)
{
XMFLOAT3 points[8];
XMVECTOR bu = XMLoadFloat3(&obb.Axis[0]);
XMVECTOR bv = XMLoadFloat3(&obb.Axis[1]);
XMVECTOR bw = XMLoadFloat3(&obb.Axis[2]);
bu = XMVectorScale(bu, obb.Extents.x);
bv = XMVectorScale(bv, obb.Extents.y);
bw = XMVectorScale(bw, obb.Extents.z);
XMStoreFloat3(&points[0], bu + bv + bw);
XMStoreFloat3(&points[1], bu + bv - bw);
XMStoreFloat3(&points[2], bu - bv + bw);
XMStoreFloat3(&points[3], bu - bv - bw);
XMStoreFloat3(&points[4], -bu + bv + bw);
XMStoreFloat3(&points[5], -bu + bv - bw);
XMStoreFloat3(&points[6], -bu - bv + bw);
XMStoreFloat3(&points[7], -bu - bv - bw);
XMFLOAT3 maxPoint(-FLT_MAX, -FLT_MAX, -FLT_MAX);
XMFLOAT3 minPoint(FLT_MAX, FLT_MAX, FLT_MAX);
for (u32 i = 0; i < 8; i++)
{
XMFLOAT3 point = points[i];
if (point.x > maxPoint.x)
maxPoint.x = point.x;
if (point.x < minPoint.x)
minPoint.x = point.x;
if (point.y > maxPoint.y)
maxPoint.y = point.y;
if (point.y < minPoint.y)
minPoint.y = point.y;
if (point.z > maxPoint.z)
maxPoint.z = point.z;
if (point.z < minPoint.z)
minPoint.z = point.z;
}
aabb->Center = obb.Center;
aabb->Extents = XMFLOAT3((maxPoint.x - minPoint.x) * 0.5f,
(maxPoint.y - minPoint.y) * 0.5f,
(maxPoint.z - minPoint.z) * 0.5f);
}
/*!
\details
Please see the details of this algorithm below URL.
http://www.scratchapixel.com/lessons/3d-basic-lessons/lesson-7-intersecting-simple-shapes/ray-box-intersection/
*/
inline
IntersectionTestResult Aabb::testIntersection(const Ray& ray,
const Vector3& inv_dir) const noexcept
{
auto t0 = (minPoint() - ray.origin()).data() * inv_dir.data();
auto t1 = (maxPoint() - ray.origin()).data() * inv_dir.data();
for (uint i = 0; i < inv_dir.size(); ++i) {
if (inv_dir[i] < 0.0)
std::swap(t0[i], t1[i]);
}
const Float tmin = zisc::max(zisc::max(t0[0], t0[1]), t0[2]);
const Float tmax = zisc::min(zisc::min(t1[0], t1[1]), t1[2]);
const auto result = (tmin <= tmax) ? IntersectionTestResult{tmin}
: IntersectionTestResult{};
return result;
}
float ATOM_TerrainPatch::computeBoundingBox (ATOM_BBox &bbox) const
{
ATOM_ASSERT(_quadtree);
float maxHeight, minHeight;
computeHeightBound (maxHeight, minHeight);
float scaleX = _quadtree->getScaleX ();
float scaleZ = _quadtree->getScaleZ ();
ATOM_Vector3f minPoint(_offsetX * scaleX, minHeight, _offsetZ * scaleZ);
ATOM_Vector3f maxPoint(_offsetX * scaleX + (_quadtree->getPatchSize() - 1) * _step * scaleX, maxHeight, _offsetZ * scaleZ + (_quadtree->getPatchSize() - 1) * _step * scaleZ);
bbox.setMax (maxPoint);
bbox.setMin (minPoint);
return (maxPoint - minPoint).getLength() * 0.5f;
}
Sphere::Sphere(const Vec3f &point, float radius, Material *m)
: Object3D(m), mCenterPoint(point), mRadius(radius)
{
#ifdef DEBUG
printf("Sphere, mCenterPoint = (x=%f,y=%f,z=%f)\n", mCenterPoint[0], mCenterPoint[1], mCenterPoint[2]);
printf("radius=%f\n", mRadius);
#endif
//Computing the bounfding Box
Vec3f minPoint(mCenterPoint.x() - mRadius,
mCenterPoint.y() - mRadius,
mCenterPoint.z() - mRadius);
Vec3f maxPoint(mCenterPoint.x() + mRadius,
mCenterPoint.y() + mRadius,
mCenterPoint.z() + mRadius);
mpBox = new BoundingBox(minPoint, maxPoint);
}
osg::Node* OSGVisualizationFactory::CreateBoundingBox(osg::Node *model, bool wireFrame)
{
if (!model)
return NULL;
const osg::MatrixList& m = model->getWorldMatrices();
osg::ComputeBoundsVisitor cbv;
model->accept( cbv );
osg::BoundingBox bboxOSG = cbv.getBoundingBox();
osg::Vec3 minV = bboxOSG._min * m.front();
osg::Vec3 maxV = bboxOSG._max * m.front();
BoundingBox bbox;
Eigen::Vector3f minPoint(minV[0],minV[1],minV[2]);
Eigen::Vector3f maxPoint(maxV[0],maxV[1],maxV[2]);
bbox.addPoint(minPoint);
bbox.addPoint(maxPoint);
return CreateBoundingBoxVisualization(bbox,wireFrame);
}
std::vector<QVector2D> HoughTransform::points(float threshold) const {
maxPoint();
std::vector<QVector2D> ret;
for (int v = 0; v < htSpace.rows; v++) {
for (int u = 0; u < htSpace.cols; u++) {
if (htSpace.at<float>(v, u) > threshold) {
QVector2D pt(u + 0.5f, v + 0.5f);
pt /= scale;
pt += bbox.minPt;
ret.push_back(pt);
}
}
}
return ret;
}
bool Player::checkChunkIntersection(Manifold& manifold) {
// Retrieve blocks for current chunk
if (m_chunkValid) {
const std::vector<glm::vec3>& blockPositions = m_currentChunk.getBlockPositions();
// Create player collider
glm::vec3 playerMinPoint(m_currentPosition.x - 0.5f, m_currentPosition.y - 1.0f, m_currentPosition.z - 0.5f);
glm::vec3 playerMaxPoint(m_currentPosition.x + 0.5f, m_currentPosition.y + 1.0f, m_currentPosition.z + 0.5f);
AABBCollider playerCollider(playerMinPoint, playerMaxPoint);
for (const glm::vec3& block : blockPositions) {
// Create AABB
glm::vec3 minPoint(block.x - 0.5f, block.y - 0.5f, block.z - 0.5f);
glm::vec3 maxPoint(block.x + 0.5f, block.y + 0.5f, block.z + 0.5f);
AABBCollider collider(minPoint, maxPoint);
// Check intersection
if (AABBCollider::checkCollision(playerCollider, collider, manifold)) {
//std::cout << "PIGNGNOFNOSFOSFNFON" << std::endl;
// ###########################################################################################
//glm::vec3 directionOfMovement(m_currentPosition - m_lastPosition);
//std::cout << "(" << directionOfMovement.x << ", " << directionOfMovement.y << ", " << directionOfMovement.z << ")" << std::endl;
//
//glm::vec3 displacement-directionO;
//
//// If im moving in the positive x
//if (directionOfMovement.x > 0) {
//
//}
//else if (directionOfMovement.x < 0) {
//
//}
//else {
// displacement.x = 0;
//}
resolveChunkIntersection(manifold, collider);
return true;
}
}
}
return false;
}
void testOrthoProjection()
{
Matrix4f proj(Matrix4f::createOrtho(-100, 100, -100, 100, -1, 1));
Vector4f center(0,0,0,1);
Vector4f minPoint(-100, -100, 1, 1);
Vector4f maxPoint(100, 100, -1, 1);
Vector4f projCenter = proj * center;
Vector4f projMinPoint = proj * minPoint;
Vector4f projMaxPoint = proj * maxPoint;
/*
std::cerr << projCenter << std::endl;
std::cerr << projMinPoint << std::endl;
std::cerr << projMaxPoint << std::endl;
*/
CPPUNIT_ASSERT(projCenter.x == 0 && projCenter.y == 0 && projCenter.z == 0);
CPPUNIT_ASSERT(projMinPoint.x == -1 && projMinPoint.y == -1 && projMinPoint.z == -1);
CPPUNIT_ASSERT(projMaxPoint.x == 1 && projMaxPoint.y == 1 && projMaxPoint.z == 1);
}
void testFrustumProjection()
{
Matrix4f proj(Matrix4f::createFrustum(-1,1,-1,1,0.1,100.0));
Vector4f minPoint(0,0,-0.1, 1);
Vector4f maxPoint(0,0,-100, 1);
minPoint = proj * minPoint;
minPoint /= minPoint.w;
maxPoint = proj * maxPoint;
maxPoint /= maxPoint.w;
const float zEpsilon = 0.1f;
/*
std::cerr << minPoint << std::endl;
std::cerr << maxPoint << std::endl;
std::cerr << zEpsilon << std::endl;
*/
CPPUNIT_ASSERT(minPoint.x == 0 && minPoint.y == 0 && fabs(minPoint.z - 0) < zEpsilon);
CPPUNIT_ASSERT(maxPoint.x == 0 && maxPoint.y == 0 && fabs(maxPoint.z - 1) < zEpsilon);
}
/**
* Compute a region that defines how the beam illuminates the given sample/can
* @param sample A reference to a sample object holding its shape
* @return A BoundingBox defining the active region
*/
Geometry::BoundingBox
RectangularBeamProfile::defineActiveRegion(const API::Sample &sample) const {
auto sampleBox = sample.getShape().getBoundingBox();
try {
const auto &envBox = sample.getEnvironment().boundingBox();
sampleBox.grow(envBox);
} catch (std::runtime_error &) {
}
// In the beam direction use the maximum sample extent other wise restrict
// the active region to the width/height of beam
const auto &sampleMin(sampleBox.minPoint());
const auto &sampleMax(sampleBox.maxPoint());
V3D minPoint, maxPoint;
minPoint[m_horIdx] = m_min[m_horIdx];
maxPoint[m_horIdx] = m_min[m_horIdx] + m_width;
minPoint[m_upIdx] = m_min[m_upIdx];
maxPoint[m_upIdx] = m_min[m_upIdx] + m_height;
minPoint[m_beamIdx] = sampleMin[m_beamIdx];
maxPoint[m_beamIdx] = sampleMax[m_beamIdx];
return Geometry::BoundingBox(maxPoint.X(), maxPoint.Y(), maxPoint.Z(),
minPoint.X(), minPoint.Y(), minPoint.Z());
}
BBox Rectangle::getBBox(){
Point minPoint(point.getX() - width/2, point.getY() - height/2);
Point maxPoint(point.getX() + width/2, point.getY() + height/2);
return BBox(minPoint,maxPoint);
}
dgInt32 dgCollisionConvexPolygon::CalculatePlaneIntersection (const dgVector& normalIn, const dgVector& origin, dgVector* const contactsOut, dgFloat32 normalSign) const
{
dgVector normal(normalIn);
dgInt32 count = 0;
dgFloat32 maxDist = dgFloat32 (1.0f);
dgFloat32 projectFactor = m_normal % normal;
if (projectFactor < dgFloat32 (0.0f)) {
projectFactor *= dgFloat32 (-1.0f);
normal = normal.Scale3 (dgFloat32 (-1.0f));
}
if (projectFactor > dgFloat32 (0.9999f)) {
for (dgInt32 i = 0; i < m_count; i ++) {
contactsOut[count] = m_localPoly[i];
count ++;
}
#ifdef _DEBUG
dgInt32 j = count - 1;
for (dgInt32 i = 0; i < count; i ++) {
dgVector error (contactsOut[i] - contactsOut[j]);
dgAssert ((error % error) > dgFloat32 (1.0e-20f));
j = i;
}
#endif
} else if (projectFactor > dgFloat32 (0.1736f)) {
maxDist = dgFloat32 (0.0f);
dgPlane plane (normal, - (normal % origin));
dgVector p0 (m_localPoly[m_count - 1]);
dgFloat32 side0 = plane.Evalue (p0);
for (dgInt32 i = 0; i < m_count; i ++) {
dgVector p1 (m_localPoly[i]);
dgFloat32 side1 = plane.Evalue (p1);
if (side0 > dgFloat32 (0.0f)) {
maxDist = dgMax (maxDist, side0);
contactsOut[count] = p0 - plane.Scale3 (side0);
count ++;
if (count > 1) {
dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32 (1.0e-8f)) {
count --;
}
}
if (side1 <= dgFloat32 (0.0f)) {
dgVector dp (p1 - p0);
dgFloat32 t = plane % dp;
dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
t = dgSign(t) * dgFloat32 (1.0e-8f);
}
contactsOut[count] = p0 - dp.Scale3 (side0 / t);
count ++;
if (count > 1) {
dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32 (1.0e-8f)) {
count --;
}
}
}
} else if (side1 > dgFloat32 (0.0f)) {
dgVector dp (p1 - p0);
dgFloat32 t = plane % dp;
dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
t = dgSign(t) * dgFloat32 (1.0e-8f);
}
contactsOut[count] = p0 - dp.Scale3 (side0 / t);
count ++;
if (count > 1) {
dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32 (1.0e-8f)) {
count --;
}
}
}
side0 = side1;
p0 = p1;
}
} else {
maxDist = dgFloat32 (1.0e10f);
dgPlane plane (normal, - (normal % origin));
dgVector p0 (m_localPoly[m_count - 1]);
dgFloat32 side0 = plane.Evalue (p0);
for (dgInt32 i = 0; i < m_count; i ++) {
dgVector p1 (m_localPoly[i]);
dgFloat32 side1 = plane.Evalue (p1);
if ((side0 * side1) < dgFloat32 (0.0f)) {
dgVector dp (p1 - p0);
dgFloat32 t = plane % dp;
dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
t = dgSign(t) * dgFloat32 (1.0e-8f);
}
contactsOut[count] = p0 - dp.Scale3 (side0 / t);
count ++;
if (count > 1) {
dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32 (1.0e-8f)) {
count --;
}
}
}
side0 = side1;
p0 = p1;
}
}
if (count > 1) {
if (maxDist < dgFloat32 (1.0e-3f)) {
dgVector maxPoint (contactsOut[0]);
dgVector minPoint (contactsOut[0]);
dgVector lineDir (m_normal * normal);
dgFloat32 proj = contactsOut[0] % lineDir;
dgFloat32 maxProjection = proj;
dgFloat32 minProjection = proj;
for (dgInt32 i = 1; i < count; i ++) {
proj = contactsOut[i] % lineDir;
if (proj > maxProjection) {
maxProjection = proj;
maxPoint = contactsOut[i];
}
if (proj < minProjection) {
minProjection = proj;
minPoint = contactsOut[i];
}
}
contactsOut[0] = maxPoint;
contactsOut[1] = minPoint;
count = 2;
}
dgVector error (contactsOut[count - 1] - contactsOut[0]);
if ((error % error) < dgFloat32 (1.0e-8f)) {
count --;
}
}
#ifdef _DEBUG
if (count > 1) {
dgInt32 j = count - 1;
for (dgInt32 i = 0; i < count; i ++) {
dgVector error (contactsOut[i] - contactsOut[j]);
dgAssert ((error % error) > dgFloat32 (1.0e-20f));
j = i;
}
if (count >= 3) {
dgVector n (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector e0 (contactsOut[1] - contactsOut[0]);
for (dgInt32 i = 2; i < count; i ++) {
dgVector e1 (contactsOut[i] - contactsOut[0]);
n += e0 * e1;
e0 = e1;
}
n = n.Scale3 (dgRsqrt(n % n));
dgFloat32 val = n % normal;
dgAssert (val > dgFloat32 (0.9f));
}
}
#endif
return count;
}
int main(int argc, char ** argv)
{
string gauss = "Gaussino";
string canny = "Canny";
string hough = "Hough";
string binarizar = "Binarizar";
string Otsu = "Otsu";
string image_name = "";
int number;
Point min, max, start;
ofstream myfile;
myfile.open("data.txt");
myfile << "ESCREVE QUALQUER COISA\n";
clock_t t1, t2, t3, t4;
double threshold1, threshold2, thres, minLength, maxGap;
bool f1, f2, f3, f4, f5, f6, f7, f8, f9;
string Result;
ostringstream convert;
//int i;
float temp;
//for (i = 1; i <= 6; i++){
//number = i;
//convert << number;
//Result = convert.str();
//image_name = "a" + Result + ".JPG";
image_name = "a2.JPG";
//number++;
//cout << number << endl;
cout << image_name;
myfile << image_name;
myfile << "\n";
t1 = clock();
f1 = false;
f2 = true;
f3 = false;
f4 = false;
f5 = false;
f6 = true;
f7 = true;
if (f7 == true){
threshold1 = 10;
threshold2 = 19;
}
f8 = false;
f9 = true;
if (f9 == true){
thres = 10;// 40
minLength = 20; //50
maxGap = 30; //80
/*
CvCapture* capture = cvCaptureFromCAM( CV_CAP_ANY );
if ( !capture ) {
fprintf( stderr, "ERROR: capture is NULL \n" );
getchar();
return -1;
}
string original = "original.jpg";
string foto ="img";
IplImage* frame = cvQueryFrame( capture );
Mat img(frame);
Mat I, I1, imge;
cvtColor(img,imge,CV_RGB2GRAY);
imge.convertTo(I, CV_8U);
equalizeHist(I,I1);
Mat aux = I1;
savePictures(I1, original, foto);
*/
//realiza a leitura e carrega a imagem para a matriz I1
// a imagem tem apenas 1 canal de cor e por isso foi usado o parametro CV_LOAD_IMAGE_GRAYSCALE
Mat lara = imread("lara.JPG", CV_LOAD_IMAGE_GRAYSCALE);
Mat I = imread(image_name, CV_LOAD_IMAGE_GRAYSCALE);
if (I.empty())
return -1;
resize(I, I, lara.size(), 1.0, 1.0, INTER_LINEAR);
Mat I1;
//Mat aux = imread(argv[1], CV_LOAD_IMAGE_GRAYSCALE);
equalizeHist(I, I1);
Mat aux, original;
aux = I1;
//ShowImage(I, I1);
// verifica se carregou e alocou a imagem com sucesso
if (I1.empty())
return -1;
// tipo Size contem largura e altura da imagem, recebe o retorno do metodo .size()
//imSize = I1.size();
// Cria uma matriz do tamanho imSize, de 8 bits e 1 canal
Mat I2 = Mat::zeros(I1.size(), CV_8UC1);
if (f2 == true) {
t2 = clock();
for (int i = 1; i < MAX_KERNEL_LENGTH; i = i + 2)
GaussianBlur(I1, I1, Size(i, i), 0, 0, BORDER_DEFAULT);
//ShowImage(aux, I1);
cout << "Guassiano tempo : ";
temp = tempo(t2);
savePictures(I1, image_name, gauss);
myfile << "Gauss: ";
myfile << temp;
myfile << "\n";
}
if (f1 == true){
t2 = clock();
binarizacao(I1, 125);
//ShowImage(aux, I1);
cout << "binarizacao : ";
temp = tempo(t2);
savePictures(I1, image_name, binarizar);
myfile << "Binarizacao: ";
myfile << temp;
myfile << "\n";
}
if (f3 == true){
t2 = clock();
inversao(I1);
cout << "inversao : ";
tempo(t2);
}
if (f4 == true){
adaptiveThreshold(I1, I1, 255, ADAPTIVE_THRESH_GAUSSIAN_C, CV_THRESH_BINARY, 7, 0);
}
if (f5 == true)
Laplacian(I1, I1, 125, 1, 1, 0, BORDER_DEFAULT);
if (f7 == true){
t2 = clock();
Canny(I1, I2, threshold1, threshold2, 3, false);
cout << "canny : ";
temp = tempo(t2);
savePictures(I2, image_name, canny);
myfile << "Canny: " + (int)(temp * 1000);
myfile << "\n";
}
if (f9 == true){
t2 = clock();
Hough(I2, aux, thres, minLength, maxGap);
cout << "hough : ";
temp = tempo(t2);
savePictures(aux, image_name, hough);
myfile << "Hough: ";
myfile << temp;
myfile << "\n";
}
if (f6 == true){
t2 = clock();
threshold_type = THRESH_BINARY;
threshold(aux, I1, 9, max_BINARY_value, threshold_type);
cout << "Threshold : ";
//savePictures(aux, image_name, Otsu);
temp = tempo(t2);
myfile << "Threshold/OTSU: ";
myfile << temp;
myfile << "\n";
}
string name = Otsu + image_name;
imwrite(name, aux);
ShowImage(I1, aux);
t2 = clock();
max = maxPoint(aux);
min = minPoint(aux);
/*start.y = (max.y + min.y) / 2;
start.x = (max.x + min.x) /2;*/
start.x = max.x;
start.y = max.y;
Point end;
end.x = start.x;
end.y = aux.size().height;
MyLine(I, start, end, image_name, 0.3);
temp = tempo(t2);
ShowImage(I, aux);
myfile << "Rota: ";
myfile << temp;
myfile << "\n";
temp = tempo(t1);
cout << "Final time : ";
myfile << "Final Time: ";
myfile << temp;
myfile << "\n";
//float angle = Angle(aux, min, 5);
//cout << angle;
}
//}
myfile.close();
//ShowImage(aux, I1);
//imwrite(argv[2], I2); // salva imagem I2 no arquivo definido pelo usuario em argv[2]
//}
return 0;
}
void addCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud)
{
//transform the cloud to the cmu_root
tf::StampedTransform stamped_tf;
Eigen::Affine3d transform;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudW(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWFiltered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWperipheral(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWFoveal(new pcl::PointCloud<pcl::PointXYZRGB>);
try{
listener->waitForTransform("cmu_root",cloud->header.frame_id,ros::Time().fromNSec(cloud->header.stamp*10e2), ros::Duration(1.0));
listener->lookupTransform("cmu_root",cloud->header.frame_id,ros::Time().fromNSec(cloud->header.stamp*10e2), stamped_tf);
}catch(tf::TransformException &ex)
{
ROS_WARN("%s",ex.what());
}
tf::transformTFToEigen(stamped_tf,transform);
pcl::transformPointCloud (*cloud, *cloudW, transform);
//if you need to assembled this addition needs to be replaced
if(final_giant->points.size()==0)
{
*final_giant=*final_giant+*cloudW;
}
else
ndtTransformAndAdd(final_giant,cloudW);
pcl::ApproximateVoxelGrid<pcl::PointXYZRGB> approximate_voxel_filter;
approximate_voxel_filter.setLeafSize (0.01, 0.01, 0.01);
approximate_voxel_filter.setInputCloud (final_giant);
approximate_voxel_filter.filter (*cloudWFiltered);
Eigen::Vector4f robot=Eigen::Vector4f(transform.translation()[0],transform.translation()[1],0,0);
Eigen::Vector4f minPoint(-4,-2,-2.5,1);
Eigen::Vector4f maxPoint(4,2,3,1);
minPoint+=robot;
maxPoint+=robot;
pcl::CropBox<pcl::PointXYZRGB> cropFilter;
cropFilter.setInputCloud (cloudWFiltered);
cropFilter.setMin(minPoint);
cropFilter.setMax(maxPoint);
cropFilter.setTransform(transform.cast<float>());
cropFilter.setNegative(false);
cropFilter.filter (*cloudWFoveal);
cropFilter.setNegative(true);
cropFilter.filter (*cloudWperipheral);
approximate_voxel_filter.setLeafSize (0.1, 0.1, 0.1);
approximate_voxel_filter.setInputCloud (cloudWperipheral);
approximate_voxel_filter.filter (*cloudWFiltered);
*final_giant=*cloudWFiltered+*cloudWFoveal;
pcl::PCLPointCloud2 output;
pcl::toPCLPointCloud2(*final_giant,output);
output.header.frame_id="map";
output.header.stamp=cloud->header.stamp;
giant_pub.publish(output);
}
/*!
\details
No detailed.
*/
inline
Float Aabb::surfaceArea() const noexcept
{
const auto range = maxPoint() - minPoint();
return 2.0 * (range[0] * range[1] + range[2] * range[1] + range[0] * range[2]);
}
void SphereCylDemo::genCylTexCoords(AXIS axis)
{
// D3DXCreate* functions generate vertices with position
// and normal data. But for texturing, we also need
// tex-coords. So clone the mesh to change the vertex
// format to a format with tex-coords.
D3DVERTEXELEMENT9 elements[64];
UINT numElements = 0;
VertexPNT::Decl->GetDeclaration(elements, &numElements);
ID3DXMesh* temp = 0;
HR(mCylinder->CloneMesh(D3DXMESH_SYSTEMMEM,
elements, gd3dDevice, &temp));
ReleaseCOM(mCylinder);
// Now generate texture coordinates for each vertex.
VertexPNT* vertices = 0;
HR(temp->LockVertexBuffer(0, (void**)&vertices));
// We need to get the height of the cylinder we are projecting the
// vertices onto. That height depends on which axis the client has
// specified that the cylinder lies on. The height is determined by
// finding the height of the bounding cylinder on the specified axis.
D3DXVECTOR3 maxPoint(-FLT_MAX, -FLT_MAX, -FLT_MAX);
D3DXVECTOR3 minPoint(FLT_MAX, FLT_MAX, FLT_MAX);
for(UINT i = 0; i < temp->GetNumVertices(); ++i)
{
D3DXVec3Maximize(&maxPoint, &maxPoint, &vertices[i].pos);
D3DXVec3Minimize(&minPoint, &minPoint, &vertices[i].pos);
}
float a = 0.0f;
float b = 0.0f;
float h = 0.0f;
switch( axis )
{
case X_AXIS:
a = minPoint.x;
b = maxPoint.x;
h = b-a;
break;
case Y_AXIS:
a = minPoint.y;
b = maxPoint.y;
h = b-a;
break;
case Z_AXIS:
a = minPoint.z;
b = maxPoint.z;
h = b-a;
break;
}
// Iterate over each vertex and compute its texture coordinate.
for(UINT i = 0; i < temp->GetNumVertices(); ++i)
{
// Get the coordinates along the axes orthogonal to the
// axis the cylinder is aligned with.
float x = 0.0f;
float y = 0.0f;
float z = 0.0f;
switch( axis )
{
case X_AXIS:
x = vertices[i].pos.y;
z = vertices[i].pos.z;
y = vertices[i].pos.x;
break;
case Y_AXIS:
x = vertices[i].pos.x;
z = vertices[i].pos.z;
y = vertices[i].pos.y;
break;
case Z_AXIS:
x = vertices[i].pos.x;
z = vertices[i].pos.y;
y = vertices[i].pos.z;
break;
}
// Convert to cylindrical coordinates.
float theta = atan2f(z, x);
float y2 = y - b; // Transform [a, b]-->[-h, 0]
// Transform theta from [0, 2*pi] to [0, 1] range and
// transform y2 from [-h, 0] to [0, 1].
float u = theta / (2.0f*D3DX_PI);
float v = y2 / -h;
// Save texture coordinates.
vertices[i].tex0.x = u;
vertices[i].tex0.y = v;
}
HR(temp->UnlockVertexBuffer());
// Clone back to a hardware mesh.
HR(temp->CloneMesh(D3DXMESH_MANAGED | D3DXMESH_WRITEONLY,
elements, gd3dDevice, &mCylinder));
ReleaseCOM(temp);
}