/**
* Find the overlap of the inputWS with the given polygon
* @param oldAxis1 :: Axis 1 bin boundaries from the input grid
* @param oldAxis2 :: Axis 2 bin boundaries from the input grid
* @param newPoly :: The new polygon to test
* @returns A list of intersection locations with weights of the overlap
*/
std::vector<Rebin2D::BinWithWeight>
Rebin2D::findIntersections(const MantidVec & oldAxis1, const MantidVec & oldAxis2,
const Geometry::ConvexPolygon & newPoly) const
{
std::vector<BinWithWeight> overlaps;
overlaps.reserve(5); // Have a guess at a posible limit
const size_t nxpoints(oldAxis1.size()-1), nypoints(oldAxis2.size()-1);
const double yn_lo(newPoly[0].Y()), yn_hi(newPoly[1].Y());
const double xn_lo(newPoly[0].X()), xn_hi(newPoly[2].X());
for(size_t i = 0; i < nypoints; ++i)
{
const double yo_lo(oldAxis2[i]), yo_hi(oldAxis2[i+1]);
// Check if there is a possibility of overlap
if( yo_hi < yn_lo || yo_lo > yn_hi ) continue;
for(size_t j = 0; j < nxpoints; ++j)
{
const double xo_lo(oldAxis1[j]), xo_hi(oldAxis1[j+1]);
// Check if there is a possibility of overlap
if( xo_hi < xn_lo || xo_lo > xn_hi ) continue;
ConvexPolygon oldPoly(xo_lo, xo_hi, yo_lo, yo_hi);
try
{
ConvexPolygon overlap = intersectionByLaszlo(newPoly, oldPoly);
overlaps.push_back(BinWithWeight(i,j,overlap.area()/oldPoly.area()));
}
catch(Geometry::NoIntersectionException &)
{}
}
}
return overlaps;
}
ConvexPolygon intersect(const ConvexPolygon &a1, const ConvexPolygon &a2)
{
ConvexPolygon toReturn;
toReturn = a1;
toReturn.append(a2);
toReturn.simplify();
return toReturn;
}
/**
* Rebin the input quadrilateral to the output grid
* @param inputQ The input polygon
* @param inputWS The input workspace containing the input intensity values
* @param i The index in the vertical axis direction that inputQ references
* @param j The index in the horizontal axis direction that inputQ references
* @param outputWS A pointer to the output workspace that accumulates the data
* @param verticalAxis A vector containing the output vertical axis bin boundaries
*/
void Rebin2D::rebinToFractionalOutput(const Geometry::Quadrilateral & inputQ,
MatrixWorkspace_const_sptr inputWS,
const size_t i, const size_t j,
RebinnedOutput_sptr outputWS,
const std::vector<double> & verticalAxis)
{
const MantidVec & X = outputWS->readX(0);
size_t qstart(0), qend(verticalAxis.size()-1), en_start(0), en_end(X.size() - 1);
if( !getIntersectionRegion(outputWS, verticalAxis, inputQ, qstart, qend, en_start, en_end)) return;
for( size_t qi = qstart; qi < qend; ++qi )
{
const double vlo = verticalAxis[qi];
const double vhi = verticalAxis[qi+1];
for( size_t ei = en_start; ei < en_end; ++ei )
{
const V2D ll(X[ei], vlo);
const V2D lr(X[ei+1], vlo);
const V2D ur(X[ei+1], vhi);
const V2D ul(X[ei], vhi);
const Quadrilateral outputQ(ll, lr, ur, ul);
double yValue = inputWS->readY(i)[j];
if (boost::math::isnan(yValue))
{
continue;
}
try
{
ConvexPolygon overlap = intersectionByLaszlo(outputQ, inputQ);
const double weight = overlap.area()/inputQ.area();
yValue *= weight;
double eValue = inputWS->readE(i)[j] * weight;
const double overlapWidth = overlap.largestX() - overlap.smallestX();
// Don't do the overlap removal if already RebinnedOutput.
// This wreaks havoc on the data.
if(inputWS->isDistribution() && inputWS->id() != "RebinnedOutput")
{
yValue *= overlapWidth;
eValue *= overlapWidth;
}
eValue *= eValue;
PARALLEL_CRITICAL(overlap)
{
outputWS->dataY(qi)[ei] += yValue;
outputWS->dataE(qi)[ei] += eValue;
outputWS->dataF(qi)[ei] += weight;
}
}
catch(Geometry::NoIntersectionException &)
{}
}
}
}
// ---------------------------------------------------------------------------------
ConvexPolygon::ConvexPolygon(const ConvexPolygon& src) {
const PolygonPoints& pnts = src.getPoints();
unsigned int size = pnts.size();
mPoints.reserve(size);
for (unsigned int x = 0; x < size; x++)
mPoints.push_back(pnts.at(x));
this->mPlane = src.getPlane();
}
/**
* Is the given polygon completely encosed by this one
* @param poly Another polygon
* @return True if the given polygon is enclosed by this, false otherwise
*/
bool ConvexPolygon::contains(const ConvexPolygon &poly) const {
// Basically just have to test if each point is inside us, this could be
// slow
const Vertex2D *current = poly.head();
for (size_t i = 0; i < poly.numVertices(); ++i) {
if (!this->contains(*current))
return false;
current = current->next();
}
return true;
}
/**
* Find the overlap of the inputWS with the given polygon
* @param oldAxis1 :: Axis 1 bin boundaries from the input grid
* @param oldAxis2 :: Axis 2 bin boundaries from the input grid
* @param newPoly :: The new polygon to test
* @returns A list of intersection locations with weights of the overlap
*/
std::vector<SofQW2::BinWithWeight>
SofQW2::findIntersections(API::MatrixWorkspace_const_sptr inputWS,
const Geometry::ConvexPolygon & newPoly) const
{
const MantidVec & oldAxis1 = inputWS->readX(0);
// Find the X boundaries
const double xn_lo(newPoly[0].X()), xn_hi(newPoly[2].X());
MantidVec::const_iterator start_it = std::upper_bound(oldAxis1.begin(), oldAxis1.end(), xn_lo);
MantidVec::const_iterator end_it = std::upper_bound(oldAxis1.begin(), oldAxis1.end(), xn_hi);
size_t start_index(0), end_index(oldAxis1.size() - 1);
if( start_it != oldAxis1.begin() )
{
start_index = (start_it - oldAxis1.begin() - 1);
}
if( end_it != oldAxis1.end() )
{
end_index = end_it - oldAxis1.begin();
}
const double yn_lo(newPoly[0].Y()), yn_hi(newPoly[1].Y());
std::vector<BinWithWeight> overlaps;
overlaps.reserve(5); // Have a guess at a possible limit
std::list<QRangeCache>::const_iterator iend = m_qcached.end();
for(std::list<QRangeCache>::const_iterator itr = m_qcached.begin();
itr != iend; ++itr)
{
for(size_t j = start_index; j < end_index; ++j)
{
const double xo_lo(oldAxis1[j]), xo_hi(oldAxis1[j+1]);
const QValues & qold = itr->qValues[j];
if( qold.upperLeft < yn_lo || qold.upperRight < yn_lo ||
qold.lowerLeft > yn_hi || qold.lowerRight > yn_hi ) continue;
Quadrilateral oldPoly(V2D(xo_lo, qold.lowerLeft), V2D(xo_hi, qold.lowerRight),
V2D(xo_hi, qold.upperRight), V2D(xo_lo, qold.upperLeft));
try
{
ConvexPolygon overlap = intersectionByLaszlo(newPoly, oldPoly);
// std::cerr << "Areas " << newPoly << " " << oldPoly << "\n";
// std::cerr << "Areas " << overlap.area() << " " << oldPoly.area() << "\n";
overlaps.push_back(BinWithWeight(itr->wsIndex,j,itr->weight*overlap.area()/oldPoly.area()));
}
catch(Geometry::NoIntersectionException &)
{}
}
}
return overlaps;
}
bool ConvexPolygon::envelopes(const ConvexPolygon &polygon) const
{
for(const Point &p : polygon.vertices())
if(!hasPoint(p))
return false;
return true;
}
size_t S4r::Pattern::Overlap(
const ConvexPolygon &poly,
std::vector<double> &value
) const{
value.resize(shape.size()+1);
value[0] = 1.;
for(size_t k = 1; k <= shape.size(); ++k){
value[k] = 0;
}
const double area = poly.Area();
const double inv_area = 1. / area;
const Vec2 org(poly.offset + poly.v[0]);
for(size_t j = 2; j < poly.v.size(); ++j){
const Vec2 u(poly.v[j-1]-poly.v[0]);
const Vec2 v(poly.v[j ]-poly.v[0]);
for(size_t k = 0; k < shape.size(); ++k){
double a = shape[k]->OverlapTriangle(org, u, v) * inv_area;
if(a > 0){
value[k+1] += a;
value[parent[k]+1] -= a;
}
}
}
size_t ret = 0;
const double tol = 4.*DBL_EPSILON * area;
for(size_t k = 0; k <= shape.size(); ++k){
if(value[k] < tol){ value[k] = 0; }
else if(value[k] > 1){ value[k] = 1; }
if(value[k] > 0){ ret++; }
}
return ret;
}
void projectPolygon(Vector axis, ConvexPolygon polygon, qreal &min, qreal &max)
{
// To project a point on an axis use the dot product
//qDebug() << "Projecting on "<< axis;
qreal d = axis.dotProduct(polygon.at(0));
min = d;
max = d;
for (int i = 0; i < polygon.size(); i++)
{
d= polygon.at(i).dotProduct (axis);
if (d < min)
min = d;
else
if (d> max) max = d;
// qDebug() << "p="<<polygon.at(i)<<" d="<<d<<" (min, max)=("<<min<<","<<max<<")";
}
}
/**
* Is the given polygon completely encosed by this one
* @param poly Another polygon
* @return True if the given polygon is enclosed by this, false otherwise
*/
bool ConvexPolygon::contains(const ConvexPolygon &poly) const {
// Basically just have to test if each point is inside us, this could be
// slow
for (size_t i = 0; i < poly.npoints(); ++i) {
if (!this->contains(poly[i]))
return false;
}
return true;
}
void CBSPTree_CollisionModel_Exporter::MakePolygonFromFace(ConvexPolygon& rPolygon, // [out]
CMapFace& rFace) // [in]
{
Vector3 avVertex[128];
int i;
for( i=0; i<rFace.GetNumVertices(); i++ )
avVertex[i] = rFace.GetVertex(i);
rPolygon.SetVertices( avVertex, rFace.GetNumVertices() );
rPolygon.SetNormal( rFace.GetPlane().normal );
rPolygon.SetDistance( rFace.GetPlane().dist );
// if( rFace.m_iPolygonIndex < 0 )
// MessageBox( NULL, "invalid polygon index", "error", MB_OK|MB_ICONWARNING );
// set the index to the corresponding 'SPolygon'
rPolygon.m_iPolygonIndex = rFace.m_iPolygonIndex;
}
bool ConvexPolygon::overlaps(const ConvexPolygon &polygon) const
{
// First a simple bound check
if(!bounds().overlaps(polygon.bounds()))
return false;
// Overlap check using separating axis theorem
for(const glm::vec2 &normal : m_normals) {
if(!test_projection(*this, polygon, normal))
return false;
}
return true;
}
void S4r::Layer::GetMaterialAverage(
const ConvexPolygon &poly,
CTensor2 &eps, CTensor2 &mu,
const std::vector<Material*> &mat,
const Material *bkmat,
std::vector<double> &value // workspace
){
size_t nmat = description.pattern.Overlap(poly, value);
if(2 != nmat){
eps = value[0] * bkmat->eps.block(0,0,2,2);
for(size_t s = 0; s < description.pattern.NumShapes(); ++s){
eps += value[s+1] * mat[description.pattern.GetShape(s).tag]->eps.block(0,0,2,2);
}
mu = value[0] * bkmat->mu.block(0,0,2,2);
for(size_t s = 0; s < description.pattern.NumShapes(); ++s){
mu += value[s+1] * mat[description.pattern.GetShape(s).tag]->mu.block(0,0,2,2);
}
}else{
// perform fancy averaging
int ip1 = -1, ip2 = -1;
for(size_t i = 0; i <= description.pattern.NumShapes(); ++i){
if(value[i] > 0){
if(ip1 >= 0){ ip2 = i; break; }
else{ ip1 = i; }
}
}
// Assert that ip1 and ip2 >= 0
CTensor2 eps1, mu1;
CTensor2 eps2, mu2;
const double fill1 = value[ip1], fill2 = value[ip2];
if(0 == ip1){
eps1 = bkmat->eps.block(0,0,2,2);
mu1 = bkmat->mu.block(0,0,2,2);
}else{
int imat = description.pattern.GetShape(ip1-1).tag;
eps1 = mat[imat]->eps.block(0,0,2,2);
mu1 = mat[imat]->mu.block(0,0,2,2);
}
{
int imat = description.pattern.GetShape(ip2-1).tag;
eps2 = mat[imat]->eps.block(0,0,2,2);
mu2 = mat[imat]->mu.block(0,0,2,2);
}
Vec2 n(description.pattern.GetShape(ip2-1).Normal(poly.ApproxCenter()));
AnisotropicAverage(n, fill1, eps1, fill2, eps2, eps);
AnisotropicAverage(n, fill1, mu1, fill2, mu2, mu );
}
}
void
GL::EnableClipPlanes(const ConvexPolygon& aPolygon, UniqueId aPolygonId)
{
MOZ_ASSERT(IsCurrent());
MOZ_ASSERT(aPolygon.NumSides() <= mMaxClipPlanes);
if (mClipPolygonId == aPolygonId) {
return;
}
if (aPolygon.IsEmpty()) {
if (!mNumClipPlanes) {
Enable(GL_CLIP_PLANE0);
} else {
for (size_t i = 1; i < mNumClipPlanes; i++) {
Disable(GL_CLIP_PLANE0 + i);
}
}
mNumClipPlanes = 1;
// We specify a single clip plane equation that fails for all vertices.
const double planeEquation[] = {0, 0, 0, -1};
ClipPlane(GL_CLIP_PLANE0, planeEquation);
mClipPolygonId = aPolygonId;
return;
}
for (size_t i = mNumClipPlanes; i < aPolygon.NumSides(); i++) {
Enable(GL_CLIP_PLANE0 + i);
}
for (size_t i = aPolygon.NumSides(); i < mNumClipPlanes; i++) {
Disable(GL_CLIP_PLANE0 + i);
}
mNumClipPlanes = aPolygon.NumSides();
for (size_t i = 0; i < aPolygon.NumSides(); i++) {
const Line& line = aPolygon.Sides()[i];
const double planeEquation[] = {line.A, line.B, 0, -line.C};
ClipPlane(GL_CLIP_PLANE0 + i, planeEquation);
}
mClipPolygonId = aPolygonId;
}
inline bool CollisionHull::testIntersection(const ConvexPolygon& other, const ruukku::GLFloatMat3& transformationOther) const {
const BoundingBox otherBoundingBoxTransformed = other.getBoundingBox().getTransformed(transformationOther);
if (!boundingBoxTransformed.intersect(otherBoundingBoxTransformed)) return false;
for (const CollisionHullEntry& entry : entryList) {
const BoundingBox entryBoundingBoxTransformed = entry.getBoundingBox().getTransformed(transformation);
if (!entryBoundingBoxTransformed.intersect(otherBoundingBoxTransformed)) continue;
for (const ConvexPolygon& polygon : entry.convexPolygonList) {
const BoundingBox polygonBoundingBoxTransformed = polygon.getBoundingBox().getTransformed(transformation);
if (!polygonBoundingBoxTransformed.intersect(otherBoundingBoxTransformed)) continue;
else if (intersectionTester.testIntersection(polygon, other, transformation, transformationOther)) return true;
}
}
return false;
}
//////////////////
// GradientLayerMesh
GradientLayerMesh::GradientLayerMesh(const Rect &rect,Vec2 axisStart,Vec2 axisEnd,const Gradient *gradient)
{
ConvexPolygon rectPol =
{
rect.GetCorner(0),
rect.GetCorner(2),
rect.GetCorner(3),
rect.GetCorner(1),
};
std::vector<float> slicePositions;
for(int i = 0;i < gradient->GetNumVertices();i++)
{
slicePositions.push_back(gradient->GetVertexPos(i));
}
Vec2 axis = axisEnd - axisStart;
axis /= axis.GetSqrLen();
float axisOffset = -Dot(axis,axisStart);
std::vector<ParallelSlice> slices = rectPol.GetParallelSlices(
axisStart,axisEnd,slicePositions.data(),(int)slicePositions.size());
for(const ParallelSlice &slice : slices)
{
int seg = slice.GetSegment();
if(seg == 0)
{
std::vector<Triangle2D> tris = slice.GetPolygon().Triangulate();
for(const Triangle2D &tri : tris)
{
mVertices.push_back(tri.mVertices[0]);
mVertices.push_back(tri.mVertices[1]);
mVertices.push_back(tri.mVertices[2]);
mColors.push_back(MakeVec4(1,0,0,1));
mColors.push_back(MakeVec4(0,1,0,1));
mColors.push_back(MakeVec4(0,0,1,1));
}
}
else if(seg == gradient->GetNumVertices())
{
std::vector<Triangle2D> tris = slice.GetPolygon().Triangulate();
for(const Triangle2D &tri : tris)
{
mVertices.push_back(tri.mVertices[0]);
mVertices.push_back(tri.mVertices[1]);
mVertices.push_back(tri.mVertices[2]);
mColors.push_back(MakeVec4(1,0,0,1));
mColors.push_back(MakeVec4(0,1,0,1));
mColors.push_back(MakeVec4(0,0,1,1));
}
}
else
{
std::vector<Triangle2D> tris = slice.GetPolygon().Triangulate();
for(const Triangle2D &tri : tris)
{
for(int i = 0;i < 3;i++)
{
mVertices.push_back(tri.mVertices[i]);
float t = Dot(axis,tri.mVertices[i]) + axisOffset;
float segT = (t - slicePositions[seg - 1]) / (slicePositions[seg] - slicePositions[seg - 1]);
Vec4 color = gradient->GetVertexColor(seg - 1) * (1 - segT) + gradient->GetVertexColor(seg) * segT;
mColors.push_back(color);
}
}
}
}
}
void ConvexPolygon::booleanDifference(const ConvexPolygon &hole, std::vector<ConvexPolygon> &list) const
{
// Special case: this polygon is entirely swallowed by the hole
if(hole.envelopes(*this))
return;
// Special case: the hole is entirely inside this polygon
if(envelopes(hole)) {
Points p1, p2;
splitPolygon(*this, hole, p1, p2);
ConvexPolygon::make(p1, list);
ConvexPolygon::make(p2, list);
return;
}
// Common case: hole intersects with this polygon.
std::vector<bool> visited(vertexCount());
std::queue<unsigned int> queue;
queue.push(0);
// Perform intersection
unsigned int oldsize = list.size();
Points poly;
while(!queue.empty()) {
int i = queue.front();
while(i < vertexCount()) {
// Stop if we've already been here
if(visited[i])
break;
visited[i] = true;
// Include point if it is not inside the hole
bool inhole = hole.hasPoint(vertex(i));
if(!inhole)
poly.push_back(vertex(i));
// Check for intersections
Point isect[2];
int isectv[2];
findIntersections(*this, i, i+1, hole, isect, isectv);
if(isectv[0] >= 0) {
// Intersection found: this is the start of a hole,
// except when this edge started inside the hole.
poly.push_back(isect[0]);
if(!inhole) {
// Start tracing the hole
int j = isectv[0];
do {
// Check for intersections
// The first hole edge may intersect with another edges
Point hisect[2];
int hisectv[2];
findIntersections(hole, j+1, j, *this, hisect, hisectv);
// There is always one intersection (the one that got us here)
if((j == isectv[0] && hisectv[1] >= 0) || (j != isectv[0] && hisectv[0] >= 0)) {
// Pick the intersection that is not the one we came in on
Point ip;
int iv;
if(hisectv[1] < 0 || glm::distance2(hisect[0],isect[0]) > glm::distance(hisect[1],isect[0])) {
ip = hisect[0];
iv = hisectv[0];
} else {
ip = hisect[1];
iv = hisectv[1];
}
queue.push(i+1);
// Avoid adding duplicate point of origin
if(glm::distance2(poly.front(), ip) > 0.0001)
poly.push_back(ip);
i = iv;
break;
} else {
// No intersections? Just add the hole vertex then
poly.push_back(hole.vertex(j));
}
if(--j < 0)
j = hole.vertexCount() - 1;
} while(j != isectv[0]);
}
}
++i;
}
// Partition the generated polygon into convex polygons
// and add them to the list.
if(poly.size() >= 3) {
try {
ConvexPolygon::make(poly, list);
} catch(const algorithm::GeometryException &e) {
// Bad polygons generated... The algorithm works well
// enough most of the time, let's just roll back the error.
int changes = list.size() - oldsize;
#ifndef NDEBUG
cerr << "booleanDifference error: " << e.what() << " (" << changes << " change(s) rolled back)\n";
#endif
while(changes-->0)
list.pop_back();
list.push_back(*this);
return;
}
}
poly.clear();
queue.pop();
}
}
void ConvexPolyhedron::cut(const Plane& plane, ConvexPolyhedron &above, ConvexPolyhedron &below) {
above.face.resize(0);
below.face.resize(0);
Array<DirectedEdge> edge;
int f;
// See if the plane cuts this polyhedron at all. Detect when
// the polyhedron is entirely to one side or the other.
//{
int numAbove = 0, numIn = 0, numBelow = 0;
bool ruledOut = false;
double d;
Vector3 abc;
plane.getEquation(abc, d);
// This number has to be fairly large to prevent precision problems down
// the road.
const float eps = 0.005f;
for (f = face.length() - 1; (f >= 0) && (!ruledOut); f--) {
const ConvexPolygon& poly = face[f];
for (int v = poly._vertex.length() - 1; (v >= 0) && (!ruledOut); v--) {
double r = abc.dot(poly._vertex[v]) + d;
if (r > eps) {
numAbove++;
} else if (r < -eps) {
numBelow++;
} else {
numIn++;
}
ruledOut = (numAbove != 0) && (numBelow !=0);
}
}
if (numBelow == 0) {
above = *this;
return;
} else if (numAbove == 0) {
below = *this;
return;
}
//}
// Clip each polygon, collecting split edges.
for (f = face.length() - 1; f >= 0; f--) {
ConvexPolygon a, b;
DirectedEdge e;
face[f].cut(plane, a, b, e);
bool aEmpty = a.isEmpty();
bool bEmpty = b.isEmpty();
//debugPrintf("\n");
if (! aEmpty) {
//debugPrintf(" Above %f\n", a.getArea());
above.face.append(a);
}
if (! bEmpty) {
//debugPrintf(" Below %f\n", b.getArea());
below.face.append(b);
}
if (! aEmpty && ! bEmpty) {
//debugPrintf(" == Split\n");
edge.append(e);
} else {
// Might be the case that the polygon is entirely on
// one side of the plane yet there is an edge we need
// because it touches the plane.
//
// Extract the non-empty _vertex list and examine it.
// If we find exactly one edge in the plane, add that edge.
const Array<Vector3>& _vertex = (aEmpty ? b._vertex : a._vertex);
int L = _vertex.length();
int count = 0;
for (int v = 0; v < L; v++) {
if (plane.fuzzyContains(_vertex[v]) && plane.fuzzyContains(_vertex[(v + 1) % L])) {
e.start = _vertex[v];
e.stop = _vertex[(v + 1) % L];
count++;
}
}
if (count == 1) {
edge.append(e);
}
}
}
if (above.face.length() == 1) {
// Only one face above means that this entire
// polyhedron is below the plane. Move that face over.
below.face.append(above.face[0]);
above.face.resize(0);
} else if (below.face.length() == 1) {
// This shouldn't happen, but it arises in practice
// from numerical imprecision.
above.face.append(below.face[0]);
below.face.resize(0);
}
if ((above.face.length() > 0) && (below.face.length() > 0)) {
// The polyhedron was actually cut; create a cap polygon
ConvexPolygon cap;
// Collect the final polgyon by sorting the edges
int numVertices = edge.length();
/*debugPrintf("\n");
for (int xx=0; xx < numVertices; xx++) {
std::string s1 = edge[xx].start.toString();
std::string s2 = edge[xx].stop.toString();
debugPrintf("%s -> %s\n", s1.c_str(), s2.c_str());
}
*/
// Need at least three points to make a polygon
debugAssert(numVertices >= 3);
Vector3 last_vertex = edge.last().stop;
cap._vertex.append(last_vertex);
// Search for the next _vertex. Because of accumulating
// numerical error, we have to find the closest match, not
// just the one we expect.
for (int v = numVertices - 1; v >= 0; v--) {
// matching edge index
int index = 0;
int num = edge.length();
double distance = (edge[index].start - last_vertex).squaredMagnitude();
for (int e = 1; e < num; e++) {
double d = (edge[e].start - last_vertex).squaredMagnitude();
if (d < distance) {
// This is the new closest one
index = e;
distance = d;
}
}
// Don't tolerate ridiculous error.
debugAssertM(distance < 0.02, "Edge missing while closing polygon.");
last_vertex = edge[index].stop;
cap._vertex.append(last_vertex);
}
//debugPrintf("\n");
//debugPrintf("Cap (both) %f\n", cap.getArea());
above.face.append(cap);
below.face.append(cap.inverse());
}
// Make sure we put enough faces on each polyhedra
debugAssert((above.face.length() == 0) || (above.face.length() >= 4));
debugAssert((below.face.length() == 0) || (below.face.length() >= 4));
}
PlayerBehaviours::PlayerBehaviours()
{
baseSpeed = 100.0;
facing = "d";
//roll properties
startRoll = false;
rolling = false;
rollCooled = true;
rollDuration = 0.5;
rollCooldown = 0.5;
rollBoost = 2.5;
//set the attack area polygons
attackScaleFactor = 2.0F;
ConvexPolygon c;
c.addPoint(6,-12);
c.addPoint(20,-30);
c.addPoint(25,-20);
c.addPoint(10,-8);
attackFrames.push_back(c);
c.clearPoints();
c.addPoint(10,-8);
c.addPoint(25,-20);
c.addPoint(30,0);
c.addPoint(12,0);
attackFrames.push_back(c);
c.clearPoints();
c.addPoint(12,0);
c.addPoint(30,0);
c.addPoint(25,20);
c.addPoint(10,8);
attackFrames.push_back(c);
c.clearPoints();
c.addPoint(10,8);
c.addPoint(25,20);
c.addPoint(20,30);
c.addPoint(6,12);
attackFrames.push_back(c);
c.clearPoints();
//set the attack properties
attackDuration = 0.15;
attackCooldown = 0.05;
startAttack = false;
attackCooled = false;
attacking = false;
attackMom = 2000;
}
ConvexPolygon ConvexPolygon::intersect(const ConvexPolygon& poly) const
{
ConvexPolygon result;
int a = 0;
int b = 0;
int aa = 0;
int ba = 0;
enum State {IN_A, IN_B, UNKNOWN};
State state = UNKNOWN;
bool firstPoint = true;
do {
const int a1 = (a + m_vertices.size() - 1) % m_vertices.size();
const int b1 = (b + poly.vertices().size() - 1) % poly.vertices().size();
const cv::Point2f vecA = m_vertices[a] - m_vertices[a1];
const cv::Point2f vecB = poly.vertices()[b] - poly.vertices()[b1];
const float cross = signedArea2(cv::Point2f(0, 0), vecA, vecB);
const float aHB = signedArea2(poly.vertices()[b1], poly.vertices()[b], m_vertices[a]);
const float bHA = signedArea2(m_vertices[a1], m_vertices[a], poly.vertices()[b]);
//std::cout << "cross=" << cross << ", aHB=" << aHB << ", bHA=" << bHA << std::endl;
std::vector<cv::Point2f> intersectionPoints;
LineSegment::IntersectResult intersectResult = LineSegment(m_vertices[a1], m_vertices[a]).intersect(LineSegment(poly.vertices()[b1], poly.vertices()[b]), intersectionPoints);
if (intersectResult == LineSegment::INTERESECTING) {
if (firstPoint && state == UNKNOWN) {
aa = 0;
ba = 0;
firstPoint = false;
}
if (aHB > 0) {
state = IN_A;
} else if (bHA > 0) {
state = IN_B;
}
assert(intersectionPoints.size() > 0);
result.addPoint(intersectionPoints[0]);
}
if ((intersectResult == LineSegment::COINCIDENT) && (intersectionPoints.size() > 0) && (vecA.dot(vecB) < 0)) {
result = ConvexPolygon(intersectionPoints);
return result;
}
if ((std::abs(cross) <= std::numeric_limits<float>::epsilon()) && (std::abs(aHB) <= std::numeric_limits<float>::epsilon()) && (std::abs(bHA) <= std::numeric_limits<float>::epsilon())) {
if (state == IN_A) {
ba++;
b = (b + 1) % poly.vertices().size();
} else {
aa++;
a = (a + 1) % m_vertices.size();
}
} else if ((std::abs(cross) <= std::numeric_limits<float>::epsilon()) && (aHB < 0) && (bHA < 0)) {
std::cout << "III" << std::endl;
result.clear();
return result;
} else if (cross >= 0) {
if (bHA > 0) {
if (state == IN_A) {
result.addPoint(m_vertices[a]);
}
aa++;
a = (a + 1) % m_vertices.size();
} else {
if (state == IN_B) {
result.addPoint(poly.vertices()[b]);
}
ba++;
b = (b + 1) % poly.vertices().size();
}
} else {
if (aHB > 0) {
if (state == IN_B) {
result.addPoint(poly.vertices()[b]);
}
ba++;
b = (b + 1) % poly.vertices().size();
} else {
if (state == IN_A) {
result.addPoint(m_vertices[a]);
}
aa++;
a = (a + 1) % m_vertices.size();
}
}
//std::cout << "a=" << a << ", b=" << b << ", aa=" << aa << ", ba=" << ba << ", state=" << state << std::endl;
} while (((aa < (int)m_vertices.size()) || (ba < (int)poly.vertices().size())) && (aa < 2 * (int)m_vertices.size()) && (ba < 2 * (int)poly.vertices().size()));
return result;
}
void ConvexPolygon::test()
{
ConvexPolygon polyA;
ConvexPolygon polyB;
polyA.addPoint(cv::Point2f(1.2647, 0.737144));
polyA.addPoint(cv::Point2f(1.09702, 2.24956));
polyA.addPoint(cv::Point2f(0.6286, 2.28678));
polyA.addPoint(cv::Point2f(0.505286, 2.26628));
polyA.addPoint(cv::Point2f(0.390494, 2.11702));
polyA.addPoint(cv::Point2f(0.300014, 1.97472));
polyA.addPoint(cv::Point2f(0.145461, 1.70306));
polyA.addPoint(cv::Point2f(0.0566236, 0.907763));
polyA.addPoint(cv::Point2f(0.0161411, 0.400278));
polyA.addPoint(cv::Point2f(0.00799566, 0.209088));
polyA.addPoint(cv::Point2f(0, 0));
polyA.addPoint(cv::Point2f(0.669949, 0.375082));
polyB.addPoint(cv::Point2f(1.45802, 0.24155));
polyB.addPoint(cv::Point2f(1.2647, 0.737144));
polyB.addPoint(cv::Point2f(0.724684, 0.74934));
polyB.addPoint(cv::Point2f(0.582521, 0.742624));
polyB.addPoint(cv::Point2f(0.450183, 0.693712));
polyB.addPoint(cv::Point2f(0.345872, 0.647083));
polyB.addPoint(cv::Point2f(0.167695, 0.558064));
polyB.addPoint(cv::Point2f(0.0652787, 0.297459));
polyB.addPoint(cv::Point2f(0.0186083, 0.131165));
polyB.addPoint(cv::Point2f(0.00921783, 0.0685148));
polyB.addPoint(cv::Point2f(0, 0));
polyB.addPoint(cv::Point2f(0.772354, 0.122908));
ConvexPolygon polyC = polyA.intersect(polyB);
polyA.print("A");
polyB.print("B");
polyC.print("C");
}
void Build(GPTexture* tex, cpSpace* a_space)
{
sprite=GPSprite(tex);
space=a_space;
ConvexPolygon polyset;
SimplePolygon BB=CreateBoundingBoxFromTexture(tex);
// polyset.addPolygon(BB);
/* SimplePolygon BB=CreateBoundingBoxFromTexture(tex);
SimplePolygon triangle1;
SimplePolygon triangle2;
triangle1.addVertex(BB.getVertex(0));
triangle1.addVertex(BB.getVertex(1));
triangle1.addVertex(BB.getVertex(2));
triangle2.addVertex(BB.getVertex(2));
triangle2.addVertex(BB.getVertex(3));
triangle2.addVertex(BB.getVertex(0));
polyset.addPolygon(triangle1);
polyset.addPolygon(triangle2);*/
polyset=CreateConvexPolySetFromTexture(tex,TYPE);
cpFloat m=1;//mass
cpFloat inertia=1;//inertia
body=cpBodyNew(m,inertia);
inertia=0;
int N_polys=polyset.getSize();
cout<<"N_polys="<<N_polys<<endl;
shape.resize(N_polys);
Nshapes+=N_polys;
for(int i=0;i<N_polys;i++)
{
SimplePolygon poly=polyset.getPolygon(i);
SDL_Surface* surf = tex->getSurface();
poly.addVector(GPVector(-surf->w/2.0,surf->h/2.0));
int N=poly.getSize();
cpVect* verts=new cpVect[N];
for(int j=0;j<N;j++)
{
// verts[N-1-j]=cpv(poly.getVertex(j).getX(),poly.getVertex(j).getY());
verts[j]=cpv(poly.getVertex(j).getX(),poly.getVertex(j).getY());
}
cpVect offset=cpvzero;
inertia+=cpMomentForPoly(m/(double)N_polys, poly.getSize(), verts, offset);
shape[i] = cpPolyShapeNew(body, poly.getSize(), verts, offset);
cpSpaceAddShape(space,shape[i]);
delete verts;
}
cout<<"inertia="<<inertia<<endl;
// cpBodySetMoment(body,inertia);
// cpBodySetMoment(body,1/*inertia*/);
SetBBInertia(m,BB,tex);
cpSpaceAddBody(space,body);
}
int main(void)
{
cpInitChipmunk(); /* Actually, that's pretty much it */
space=cpSpaceNew();
space->gravity=cpv(0,-100);
space->elasticIterations=7;
cpFloat dt=0.008;
cpBody* staticBody = cpBodyNew(INFINITY, INFINITY);
cpShape* bottom=cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(320,-240), 5.0f);
cpShape* leftside=cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(-320,240), 5.0f);
cpShape* rightside=cpSegmentShapeNew(staticBody, cpv(320,-240), cpv(320,240), 5.0f);
cpSpaceAddStaticShape(space,bottom);
cpSpaceAddStaticShape(space,leftside);
cpSpaceAddStaticShape(space,rightside);
cpFloat dim=10;
int count=1000;
cpSpaceResizeStaticHash(space, dim, count);
cpSpaceResizeActiveHash(space, dim, count);
GPEngine* engine = GPEngine::getInstance();
engine->initAll();
window = GPWindow::getInstance();
window->setRenderMode(GPWindow::GP_OPENGL);
window->setWindowTitle("Gamepower 2D Testing App");
window->createWindow(640, 480, 24);
window->setGraphicsDefaults();
GPKeyboardHandler* handler = new GPKeyboardHandler();
handler->registerKeyAction(GPKeyEvent::GP_KEY_ESCAPE, new ExitAction());
handler->registerKeyAction(GPKeyEvent::GP_KEY_SPACE, new TogglePhysics());
GPMouseHandler* mhandler = new GPMouseHandler();
mhandler->registerMouseAction(new MouseAction());
GPInputProcessor* proc = new GPInputProcessor();
proc->addHandler(handler);
proc->addHandler(mhandler);
cam = new GPGameCamera(window->getHeight() / 2);
cam->setPosition(0.0f, 0.0f);
texSprite[0] = new GPTexture("susi.png");
texSprite[1] = new GPTexture("debin.png");
texSprite[2] = new GPTexture("fidori.png");
texSprite[3] = new GPTexture("genti.png");
texSprite[4] = new GPTexture("lini.png");
texSusi = new GPTexture("susi.png");
texMissile = new GPTexture("missile.png");
texLeaves = new GPTexture("iteam_leaves_animation.png");
ConvexPolygon polySprite[5];
ConvexPolygon polyMissile;
cout<<"=================="<<endl;
for(int l_TYPE=0;l_TYPE<4;l_TYPE++)
{
cout<<"++++++++++++++++"<<endl;
cout<<"Susi:"<<endl;
polySprite[0]=CreateConvexPolySetFromTexture(texSprite[0],l_TYPE);
cout<<"Debin:"<<endl;
polySprite[1]=CreateConvexPolySetFromTexture(texSprite[1],l_TYPE);
cout<<"Fidori:"<<endl;
polySprite[2]=CreateConvexPolySetFromTexture(texSprite[2],l_TYPE);
cout<<"Genti:"<<endl;
polySprite[3]=CreateConvexPolySetFromTexture(texSprite[3],l_TYPE);
cout<<"Lini:"<<endl;
polySprite[4]=CreateConvexPolySetFromTexture(texSprite[4],l_TYPE);
cout<<"Missile:"<<endl;
polyMissile=CreateConvexPolySetFromTexture(texMissile,l_TYPE);
}
cout<<"=================="<<endl;
ConvexPolygon polyLeaves=CreateConvexPolySetFromTexture(texLeaves,TYPE);
ConvexPolygon polySusi=CreateConvexPolySetFromTexture(texSusi,TYPE);
polySusi.setPosition(0,0);
polyMissile.setPosition(100,0);
polyLeaves.setPosition(0,100);
for(int i=0;i<5;i++) cout<<"polySprite[i].getSize()="<<polySprite[i].getSize()<<endl;
cout<<"polySusi.getSize()="<<polySusi.getSize()<<endl;
cout<<"polyMissile.getSize()="<<polyMissile.getSize()<<endl;
cout<<"polyLeaves.getSize()="<<polyLeaves.getSize()<<endl;
class_testing(texSusi,space);
// vector <GP_chipmunk_combo> sprSusi1;
// sprSusi1.push_back(GP_chipmunk_combo(texSusi,space));
// sprSusi1[sprSusi1.size()-1].setPosition(0.0f, 0.0f);
// GP_chipmunk_combo S(texSusi,space,"normal element");
// S.setPosition(0,0);
// S.setRotation_Z(0);
// S.sprite.setRotCenter(64,-64,0);
// cout<<"S.sprite.getRotCenter_X()="<<S.sprite.getRotCenter_X()<<endl;
// cout<<"S.sprite.getRotCenter_Y()="<<S.sprite.getRotCenter_Y()<<endl;
// cout<<"S.sprite.getRotCenter_Z()="<<S.sprite.getRotCenter_Z()<<endl;
bottom->e=1.0;
bottom->u=0.0;
/* S.SetElasticity(0.0);
S.SetFriction(0.0);*/
/* cout<<"S elasticity="<<S.shape->e<<endl;
cout<<"S friction="<<S.shape->u<<endl;*/
cout<<"bottom elasticity="<<bottom->e<<endl;
cout<<"bottom friction="<<bottom->u<<endl;
// GP_chipmunk_combo B=S;
/* B.setPosition(100,100);
B.setRotation_Z(46);*/
// GP_chipmunk_combo C=S;
/* C.setPosition(200,200);
C.setRotation_Z(180);*/
// GP_chipmunk_combo D=S;
/* D.setPosition(300,300);
D.setRotation_Z(270);*/
Uint32 start = 0;
start = SDL_GetTicks();
bool random_susi=true;
GPFont* font = new GPFont("font.ttf", 12);
GPText* text1 = new GPText(font);
text1->setRenderType(GPText::GP_RENDER_NICEST);
text1->setX(0);
text1->setY(0);
text1->setZ(12);
text1->setText("Hi 1");
int frames=0;
GPTimer timer;
timer.start();
while(!quit) {
proc->pollEvents();
window->clearScreen();
// S.draw();
// B.draw();
// C.draw();
// D.draw();
int FPS;
int milliseconds=timer.getTicks();
// cout<<timer.getTicks()<<endl;
if(milliseconds>1000) {
FPS=frames/(double)(milliseconds/1000.0);
/* cout<<"frames="<<frames<<endl;
cout<<"milliseconds="<<milliseconds<<endl;*/
cout<<"FPS="<<FPS<<" SpriteVector.size()="<<SpriteVector.size()<<" Nshapes="<<Nshapes<<endl;
frames=0;
timer.reset();
}
// text1->setText("FPS=%i",FPS);
// text1->draw();
// polySusi.draw();
// polyMissile.draw();
// polyLeaves.draw();
// Uint32 T=SDL_GetTicks() - start;
// if(T>1000 && random_susi)
// {
// random_susi=false;
// int x = rand() % 640 - 320;
// int y = rand() % 480 - 240;
// cout << "Timer: " << T << " -> ("<<x<<","<<y<<")"<<endl;
// SpriteVector.push_back(GP_chipmunk_combo(texSusi,space,"vector element"));
// SpriteVector[SpriteVector.size()-1].setPosition(x,y);
// start = SDL_GetTicks();
// }
for(unsigned int i=0;i<SpriteVector.size();i++)
{
SpriteVector[i].draw();
}
/* drawLine(100,240,100,-240);
drawLine(320,100,-320,100);*/
drawLine(-320,-240,320,-240);//bottom
drawLine(-320,-240,-320,240);//left
drawLine(320,-240,320,240);//right
// drawLine(0,0,320,240);//diagonal
// drawLine(0,0,320,0);//Xbottom
// drawLine(0,0,0,240);//Ybottom
window->sync();
if(physics) cpSpaceStep(space,dt);
frames++;
}
delete font;
delete text1;
engine->shutdownAll();
atexit(clean_physics);
return(0);
}
void ConstraintBuilder::processEnvironment() {
//pre-compute allowed positions, max and min thetas
map->precomputeAllowedPositions(allowedStates,stateIntervals);
double theta_increment = 2*M_PI / (double) N_THETA_INCREMENTS;
double len = 0.5;
cv::Point pixel;
size_t x,y;
//go through all poses in the trajectory
for( size_t i=0; i<trajectory.size(); i++) {
//create a polygon with a seed set to the trajectory point
ConvexPolygon poly;
poly.seed(0) = trajectory[i].poseX;
poly.seed(1) = trajectory[i].poseY;
map->world2pixel(poly.seed, pixel);
if(pixel.x < 0 || pixel.y < 0 || pixel.x >= map->sizeY || pixel.y>= map->sizeX) {
cout<<"ERROR in polygon creation - trajectory goes out of map at "<<poly.seed.transpose()<<"\n";
return;
}
x = pixel.y;
y = pixel.x;
cout<<"polygon seed at "<<x<<" "<<y<<endl;
if(!allowedStates(x,y)) {
cout<<"ERROR in polygon creation - trajectory goes through obstacle space at "<<poly.seed.transpose()<<"\n";
return;
}
int idx = trajectory[i].poseT / theta_increment;
if(!stateIntervals(x,y).allowed_theta_phi[idx][0]) {
cout<<"ERROR in polygon creation - trajectory goes through obstacle space at "
<<poly.seed.transpose()<<" with theta wrong at "<<idx<<" \n";
return;
}
size_t idxmin = (idx == 0) ? 0 : idx-1;
size_t idxmax = (idx == N_THETA_INCREMENTS-1) ? idx : idx+1;
int nMoves = 0;
vector<pair<size_t,size_t> > best_endpoints, current_endpoints, eps;
pair<size_t, size_t> ul (x-5,y-5), ll(x-5,y+5), lr(x+5,y+5), ur(x+5,y-5);
int cl1, cl2;
double mindist, mindist2, dist;
Eigen::Vector2d vec;
size_t xprev, yprev;
int MAX_XDIST = 10, MAX_YDIST = 10;
best_endpoints.push_back(ul);
best_endpoints.push_back(ll);
best_endpoints.push_back(lr);
best_endpoints.push_back(ur);
vector<int> indexes_to_consider;
if(i>=1) {
indexes_to_consider.push_back(i-1);
}
//search direction towards next
if(i<trajectory.size()-1) {
indexes_to_consider.push_back(i+1);
}
while (nMoves<50) {
current_endpoints = best_endpoints;
//search direction towards previous
for(int sn =0; sn<indexes_to_consider.size(); sn++) {
int next = indexes_to_consider[sn];
vec<<trajectory[next].poseX,trajectory[next].poseY;
map->world2pixel(vec, pixel);
xprev = pixel.y;
yprev = pixel.x;
//find the two closest points in best endpoints
mindist=INT_MAX;
for(int q =0; q<current_endpoints.size(); q++) {
dist = sqrt( pow((double)current_endpoints[q].first-xprev,2)
+ pow((double)current_endpoints[q].second-yprev,2));
//cout<<"q "<<q<<" dist "<<dist<<" to ("<<xprev<<","<<yprev<<")\n";
if(dist<mindist) {
mindist = dist;
cl1 = q;
}
}
int prev_cl1, next_cl1;
prev_cl1 = cl1 == 0 ? current_endpoints.size()-1 : cl1-1;
next_cl1 = cl1 == current_endpoints.size()-1 ? 0 : cl1+1;
double dist1 = sqrt( pow((double)current_endpoints[prev_cl1].first-xprev,2)
+ pow((double)current_endpoints[prev_cl1].second-yprev,2));
double dist2 = sqrt( pow((double)current_endpoints[next_cl1].first-xprev,2)
+ pow((double)current_endpoints[next_cl1].second-yprev,2));
cl2 = dist1 < dist2 ? prev_cl1 : next_cl1;
//move them in the direction of xprev yprev
int dx1,dy1,dx2,dy2,dx3,dy3;
dx1 = current_endpoints[cl1].first - xprev;// ? -1 : current_endpoints[cl1].first < xprev ? 1 : 0;
dy1 = current_endpoints[cl1].second - yprev;// ? -1 : current_endpoints[cl1].second < yprev ? 1 : 0;
dx2 = current_endpoints[cl2].first - xprev;// ? -1 : current_endpoints[cl2].first < xprev ? 1 : 0;
dy2 = current_endpoints[cl2].second - yprev;// ? -1 : current_endpoints[cl2].second < yprev ? 1 : 0;
dx1 = dx1 > 0 ? -1 : 1;
dy1 = dy1 > 0 ? -1 : 1;
dx2 = dx2 > 0 ? -1 : 1;
dy2 = dy2 > 0 ? -1 : 1;
current_endpoints[cl1].first += dx1;
current_endpoints[cl1].second += dy1;
current_endpoints[cl2].first += dx2;
current_endpoints[cl2].second += dy2;
// cout<<"d: "<<dx1<<" "<<dy1<<", "<<dx2<<" "<<dy2<<" eps "<<cl1<<" "<<cl2<<endl;
dx3 = current_endpoints[cl1].first - current_endpoints[cl2].first;
dy3 = current_endpoints[cl1].second - current_endpoints[cl2].second;
if(abs(dx3) < MAX_XDIST) {
//move away from each other
if(dx3 > 0) {
dx1 =2;
dx2 =-2;
} else {
dx2 =2;
dx1 =-2;
}
}
if(abs(dy3) < MAX_YDIST) {
//move away from each other
if(dy3 > 0) {
dy1 =2;
dy2 =-2;
} else {
dy2 =2;
dy1 =-2;
}
}
//cout<<"d: "<<dx1<<" "<<dy1<<", "<<dx2<<" "<<dy2<<" eps "<<cl1<<" "<<cl2<<endl;
current_endpoints[cl1].first += dx1;
current_endpoints[cl1].second += dy1;
current_endpoints[cl2].first += dx2;
current_endpoints[cl2].second += dy2;
}
bool doGrow = true;
//trace polygon as well
for(int q = 1; q<current_endpoints.size(); q++) {
//cout<<current_endpoints[q-1].first<<" "<<current_endpoints[q-1].second<<" to "<<current_endpoints[q].first<<" "<<current_endpoints[q].second;
if(!traceLine(current_endpoints[q-1].first,current_endpoints[q-1].second,
current_endpoints[q].first,current_endpoints[q].second,idxmin,idxmax)) {
doGrow = false;
break;
}
//cout<<" OK\n";
}
if(!doGrow) break;
//cout<<current_endpoints.back().first<<" "<<current_endpoints.back().second<<" to "<<current_endpoints.front().first<<" "<<current_endpoints.front().second;
if(!traceLine(current_endpoints.back().first,current_endpoints.back().second,
current_endpoints.front().first,current_endpoints.front().second,idxmin,idxmax)) {
break;
}
//cout<<" OK\n";
best_endpoints = current_endpoints;
nMoves++;
}
//dummy 0.5 m in each direction
/* Eigen::Vector2d s;
Eigen::Vector2d ul, ll, lr, ur;
ul<<-len/2,len/2;
ll<<-len/2,-len/2;
lr<<len/2,-len/2;
ur<<len/2,len/2;
s = poly.seed + ul;
poly.endpoints.push_back(s);
s = poly.seed + ll;
poly.endpoints.push_back(s);
s = poly.seed + lr;
poly.endpoints.push_back(s);
s = poly.seed + ur;
poly.endpoints.push_back(s);
*/
//go through best_endpoints, compute vector position and add in polygon
for(int q=0; q<best_endpoints.size(); q++) {
pixel.x = best_endpoints[q].second;
pixel.y = best_endpoints[q].first;
map->pixel2world(pixel,vec);
poly.endpoints.push_back(vec);
}
poly.theta_min = -1;
poly.theta_max = 1;
poly.phi_min = -1;
poly.phi_max = 1;
poly.v_min = -1;
poly.v_max = 1;
poly.w_min = -1;
poly.w_max = 1;
poly.calculateMatrixForm();
polygons.push_back(poly);
//cout<<"added poly at "<<poly.seed<<endl;
}
}
/**
* Compute the polygon that defines the intersection between two
* other concex polygons using the method of chasing edges
* @param P :: A reference to the first polygon
* @param Q :: A reference to the second polygon
* @returns A new polygon defining the region of intersection
*/
ConvexPolygon intersectionByORourke(const ConvexPolygon &P, const ConvexPolygon &Q)
{
const size_t nverts_p(P.numVertices()), nverts_q(Q.numVertices());
const V2D origin(0.0,0.0);
size_t count_p(0), count_q(0); // Number of vertices visited
unsigned int inflag(Unknown);
bool firstPoint(true);
// Avoid vertex list reallocations
Vertex2DList originAB(3);
originAB[0] = origin;
Vertex2DList aHB(3);
Vertex2DList bHA(3);
// Final list
Vertex2DList intersectList;
do
{
size_t pi(0), pim1(0), qi(0), qim1(0);
// Compute a vector between the previous point in the direction of the next
pim1 = (pi + nverts_p - 1) % nverts_p;
qim1 = (qi + nverts_q - 1) % nverts_q;
V2D edge_p = P[pi] - P[pim1];
V2D edge_q = Q[qi] - Q[qim1];
// Orientations
originAB[1] = edge_p;
originAB[2] = edge_q;
int cross = ConvexPolygon(originAB).orientation();
aHB[0] = Q[qim1];
aHB[1] = Q[qi];
aHB[2] = P[pi];
int aHB_dir = ConvexPolygon(aHB).orientation();
bHA[0] = P[pim1];
bHA[1] = P[pi];
bHA[2] = Q[qi];
int bHA_dir = ConvexPolygon(bHA).orientation();
// Test for line intersection
V2D intersect;
unsigned int type = intersection(P[pim1],P[pi], Q[qim1],Q[qi], intersect);
if( type == 1 || type == 2 )
{
if( inflag == Unknown && firstPoint )
{
count_p = count_q = 0;
firstPoint = false;
}
if( aHB_dir > 0 ) inflag = Pin;
else if( bHA_dir > 0 ) inflag = Qin;
else {};
intersectList.insert(intersect);
}
// Deal with advance of indices
/* Special case: A & B overlap and oppositely oriented. */
if ( type == 3 && edge_p.scalar_prod(edge_q) < 0.0 )
{
throw std::runtime_error("Single segment intersection");
}
/* Special case: A & B parallel and separated. */
else if ( cross == 0 && (aHB_dir < 0) && (bHA_dir < 0) )
{
throw std::runtime_error("AxB=0 and both are left-hand oriented so no intersection");
}
/* Special case: A & B collinear. */
else if ( cross == 0 && (aHB_dir == 0) && (bHA_dir == 0) )
{
/* Advance but do not output point. */
if ( inflag == Pin )
qi = advanceVertex(qi, count_q, nverts_q, false, Q[qi], intersectList);
else
pi = advanceVertex(pi, count_p, nverts_p, false, P[pi], intersectList);
}
/* Generic cases. */
else if ( cross >= 0 )
{
if ( bHA_dir > 0)
pi = advanceVertex(pi, count_p, nverts_p, inflag == Pin, P[pi], intersectList);
else
qi = advanceVertex(qi, count_q, nverts_q, inflag == Qin, Q[qi], intersectList);
}
else /* if ( cross < 0 ) */
{
if ( aHB_dir > 0)
qi = advanceVertex(qi, count_q, nverts_q, inflag == Qin, Q[qi], intersectList);
else
pi = advanceVertex(pi, count_p, nverts_p, inflag == Pin, P[pi], intersectList);
}
}
while( (count_p < nverts_p || count_q < nverts_q) &&
(count_p < 2*nverts_p) && (count_q < 2*nverts_q) );
if( intersectList.size() < 3 )
{
throw std::runtime_error("Intersection points do not form a bounded polygon.");
}
return ConvexPolygon(intersectList);
}
PolygonCollisionResult polygonCollision(ConvexPolygon polygonA,
ConvexPolygon polygonB, Vector velocity)
{
PolygonCollisionResult result;
result.intersect = true;
result.willIntersect = true;
int edgeCountA = polygonA.size();
int edgeCountB = polygonB.size();
qreal minIntervalDistance = 1000000000;
QPointF translationAxis;
QPointF edge;
/*
qDebug() << "A: ";
for (int k=0; k<edgeCountA;k++)
qDebug() <<polygonA.at(k);
qDebug() << "B: ";
for (int k=0; k<edgeCountB;k++)
qDebug() <<polygonB.at(k);
qDebug() ;
*/
// Loop through all the edges of both polygons
for (int i=0;i<edgeCountA + edgeCountB;i++)
{
if (i< edgeCountA)
{
// Loop through polygon A
if (i<edgeCountA-1)
edge = QPointF (
polygonA.at(i+1).x()-polygonA.at(i).x(),
polygonA.at(i+1).y()-polygonA.at(i).y());
else
edge = QPointF (
polygonA.at(0).x()-polygonA.at(i).x(),
polygonA.at(0).y()-polygonA.at(i).y());
} else
{
// Loop through polygon B
if (i < edgeCountA +edgeCountB -1 )
edge = QPointF (
polygonB.at(i-edgeCountA+1).x() - polygonB.at(i-edgeCountA).x(),
polygonB.at(i-edgeCountA+1).y() - polygonB.at(i-edgeCountA).y());
else
edge = QPointF (
polygonB.at(0).x() - polygonB.at(i-edgeCountA).x(),
polygonB.at(0).y() - polygonB.at(i-edgeCountA).y());
}
// ===== 1. Find if the polygons are currently intersecting =====
// Find the axis perpendicular to the current edge
Vector axis (-edge.y(), edge.x());
axis.normalize();
// Find the projection of the polygon on the current axis
qreal minA = 0; qreal minB = 0; qreal maxA = 0; qreal maxB = 0;
projectPolygon(axis, polygonA, minA, maxA);
projectPolygon(axis, polygonB, minB, maxB);
// Check if the polygon projections are currentlty intersecting
qreal d = intervalDistance(minA, maxA, minB, maxB);
if (d > 0) result.intersect = false;
// ===== 2. Now find if the polygons *will* intersect =====
// Project the velocity on the current axis
qreal velocityProjection = axis.dotProduct(velocity);
// Get the projection of polygon A during the movement
if (velocityProjection < 0)
minA += velocityProjection;
else
maxA += velocityProjection;
// Do the same test as above for the new projection
// d = intervalDistance(minA, maxA, minB, maxB);
//if (d > 0) result.willIntersect = false;
/*
qDebug() <<" ";
qDebug() << "edge="<<edge<<" ";
qDebug() <<"axis="<<axis<<" ";
qDebug() <<"dA=("<<minA<<","<<maxA<<") dB=("<<minB<<","<<maxB<<")";
qDebug() <<" d="<<d<<" ";
//qDebug() <<"minD="<<minIntervalDistance<<" ";
qDebug() <<"int="<<result.intersect<<" ";
//qDebug() <<"wint="<<result.willIntersect<<" ";
//qDebug() <<"velProj="<<velocityProjection<<" ";
qDebug() ;
*/
if (result.intersect )// || result.willIntersect)
{
// Check if the current interval distance is the minimum one. If so
// store the interval distance and the current distance. This will
// be used to calculate the minimum translation vector
if (d<0) d=-d;
if (d < minIntervalDistance) {
minIntervalDistance = d;
//translationAxis = axis;
//qDebug() << "tAxix="<<translationAxis;
//QPointF t = polygonA.Center - polygonB.Center;
//QPointF t = polygonA.at(0) - polygonB.at(0);
//if (dotProduct(t,translationAxis) < 0)
// translationAxis = -translationAxis;
}
}
}
// The minimum translation vector
// can be used to push the polygons appart.
if (result.willIntersect)
result.minTranslation =
translationAxis * minIntervalDistance;
return result;
}
