CAL_STATE pidHysterisis(int group) {
if (RunEvery(&getPidGroupDataTable(group)->timer) > 0) {
Print_Level l = getPrintLevel();
//setPrintLevelInfoPrint();
float boundVal = 150.0;
float extr = GetPIDPosition(group);
if (bound(0, extr, boundVal, boundVal)) {// check to see if the encoder has moved
//we have not moved
// println_I("NOT moved ");p_fl_I(extr);
if (getPidGroupDataTable(group)->calibration.state == forward) {
incrementHistoresis(group);
} else if (getPidGroupDataTable(group)->calibration.state == backward) {
decrementHistoresis(group);
}
int historesisBound = 25;
if (getPidGroupDataTable(group)->config.lowerHistoresis < (-historesisBound) &&
getPidGroupDataTable(group)->calibration.state == backward) {
println_E("Backward Motor seems damaged, more then counts of historesis #");
p_int_I(group);
getPidGroupDataTable(group)->calibration.state = forward;
}
if (getPidGroupDataTable(group)->config.upperHistoresis > (historesisBound) &&
getPidGroupDataTable(group)->calibration.state == forward) {
println_E("Forward Motor seems damaged, more then counts of historesis #");
p_int_I(group);
getPidGroupDataTable(group)->calibration.state = done;
}
} else {
pidReset(group, 0);
setOutput(group, 0);
println_E("Moved ");
p_fl_E(extr);
if (getPidGroupDataTable(group)->calibration.state == forward) {
println_I("Backward Calibrated for link# ");
p_int_I(group);
getPidGroupDataTable(group)->calibration.state = backward;
} else {
println_I("Calibration done for link# ");
p_int_I(group);
getPidGroupDataTable(group)->calibration.state = done;
float offset = .9;
getPidGroupDataTable(group)->config.lowerHistoresis *= offset;
getPidGroupDataTable(group)->config.upperHistoresis *= offset;
calcCenter(group);
}
}
if (getPidGroupDataTable(group)->calibration.state == forward) {
setOutput(group, 1.0f);
} else if (getPidGroupDataTable(group)->calibration.state == backward) {
setOutput(group, -1.0f);
}
setPrintLevel(l);
}
if (getPidGroupDataTable(group)->calibration.state == done)
SetPIDCalibrateionState(group, CALIBRARTION_DONE);
return getPidGroupDataTable(group)->calibration.state;
}
/**
Scale box from center coordinate.
*/
inline void scaleFromCenter( float scaleFactor )
{
float scaleFactorMinus = scaleFactor - 1.f;
Vector3 center = calcCenter();
m_Max += ( m_Max - center ) * scaleFactorMinus;
m_Min += ( m_Min - center ) * scaleFactorMinus;
}
void Object::setPosition(float x, float y)
{
this->x = x;
this->y = y;
calcCenter();
}
Object::Object(float X, float Y, int w, int h,
float accelerationX, float accelerationY,
int status, float hitBoxFactor, std::string bitmapFileLoc)
{
x = X;
y = Y;
this->w = w;
this->h = h;
accelX = accelerationX;
accelY = accelerationY;
this->status = status;
this->hitboxFactor = hitBoxFactor;
name = OBJECT;
calcCenter();
if (al_is_system_installed())
{
bitmap = al_load_bitmap(bitmapFileLoc.c_str());
if (!bitmap)
{
setBit(ER_ERROR);
generate_error_bitmap();
}
} else
{
setBit(ER_ERROR);
setBit(ER_INVALID_STATE);
bitmap = NULL;
}
}
short int legalizeEdge(Point * p, Triangle * T){
short int ut, ua;
Triangle * A;
Point * center;
for(ut=0; ut<3 && T->points[ut] != p; ++ut);
if(ut==3) return -1;
ADJACENT(T, T->points[NEXT(ut)], T->points[PREV(ut)], A);
if(!A) return 1;
for(ua=0; ua<3 && IN_TRIA(T, A->points[ua]); ua++);
center = calcCenter(T);
if(!center) return -2;
if(DISQR(A->points[ua], center) >= DISQR(center, P0)){
free(center);
return 1;
}
free(center);
if((ua=swap(T, A))<0) return -3;
if((ua=legalizeEdge(p, T))<0) return -ua;
if((ua=legalizeEdge(p, A))<0) return -ua;
return 1;
}
void
MeshElementTable::createTableEntry(int int_id, meshElementCode elem_code,
const int* parent_pair_ids,
int nof_nodes, const int* entry_node_ids)
{
int i;
elementCodes[int_id] = elem_code;
// Edge ids
if ( edgeIds != NULL ) {
int nof_edges = MeshElementDesc[elem_code][DESC_NOF_EDGES];
edgeIds[int_id] = new int[nof_edges];
for (i = 0; i < nof_edges; i++) {
edgeIds[int_id][i] = NO_INDEX;
}
}
// Parent ids
if (parentIds != NULL && parent_pair_ids != NULL ) {
if ( parentIds[int_id] == NULL ) {
parentIds[int_id] = new int[2];
}
for (i = 0; i < 2; i++) {
parentIds[int_id][i] = parent_pair_ids[i];
}
}
// Node ids
if ( nodeIds != NULL && nof_nodes > 0 && entry_node_ids != NULL) {
nodeIds[int_id] = new int[nof_nodes];
for (i = 0; i < nof_nodes; i++) {
nodeIds[int_id][i] = entry_node_ids[i];
}
}
// Neighbor element info
if (neighborIds != NULL) {
int nof_sub_elems = MeshElementDesc[elem_code][DESC_NOF_BNDR_ELEMS];
neighborIds[int_id] = new int[nof_sub_elems];
for (i = 0; i < nof_sub_elems; i++) {
neighborIds[int_id][i] = UNSET_INDEX;
}
}
// Calc center and max radius
calcCenter(int_id);
}
/**
Scale from center non uniformly.
*/
inline void scaleNonUniformFromCenter( const Vector3& scaleFactor )
{
Vector3 scaleFactorMinusOne = scaleFactor - Vector3( 1.f, 1.f, 1.f );
Vector3 center = calcCenter();
m_Max.x += ( m_Max.x - center.x ) * scaleFactorMinusOne.x;
m_Max.y += ( m_Max.y - center.y ) * scaleFactorMinusOne.y;
m_Max.z += ( m_Max.z - center.z ) * scaleFactorMinusOne.z;
m_Min.x += ( m_Min.x - center.x ) * scaleFactorMinusOne.x;
m_Min.y += ( m_Min.y - center.y ) * scaleFactorMinusOne.y;
m_Min.z += ( m_Min.z - center.z ) * scaleFactorMinusOne.z;
}
ofVec3f ofxPhysicalOBJFace::moveOutFrom(ofVec3f p, float amount) {
calcCenter();
ofVec3f dir = center - p;
dir.normalize();
dir*= amount;
return dir;
}
void ofxPhysicalOBJFace::rotate(ofVec3f axis) {
// static float rotation = 0;
calcCenter();
// rotation+= 50;
rotation = 15;
for (ofVec3f &v : vertices) {
v.rotate(rotation, center, axis);
}
}
/**
* @param points vector of points
* @param r Distance
*/
DistanceConnectCluster(const std::vector<T> &points,const X& r){
distance = r;
label.assign(points.size(),-1);
int clusternr =0;
for(size_t i=0;i<points.size();i++){
if(label[i] == -1){
label[i]= clusternr;
clusternr++;
}
doPoint(points,i);
}
calcCenter(points,clusternr);
}
void
Bbox::fromPointSet(std::vector<Vector> pointset)
{
std::vector<Vector> sorted_pointset;
sorted_pointset = sortedVectorArray(pointset,0);
min.x = sorted_pointset.front().x;
max.x = sorted_pointset.back().x;
sorted_pointset = sortedVectorArray(pointset,1);
min.y = sorted_pointset.front().y;
max.y = sorted_pointset.back().y;
sorted_pointset = sortedVectorArray(pointset,2);
min.z = sorted_pointset.front().z;
max.z = sorted_pointset.back().z;
calcCenter();
}
float DiffTraversal::View::computePriority(const EntityItemPointer& entity) const {
if (!entity) {
return PrioritizedEntity::DO_NOT_SEND;
}
if (!usesViewFrustums()) {
return PrioritizedEntity::WHEN_IN_DOUBT_PRIORITY;
}
bool success = false;
auto cube = entity->getQueryAACube(success);
if (!success) {
return PrioritizedEntity::WHEN_IN_DOUBT_PRIORITY;
}
auto center = cube.calcCenter(); // center of bounding sphere
auto radius = 0.5f * SQRT_THREE * cube.getScale(); // radius of bounding sphere
auto priority = PrioritizedEntity::DO_NOT_SEND;
for (const auto& frustum : viewFrustums) {
auto position = center - frustum.getPosition(); // position of bounding sphere in view-frame
float distance = glm::length(position); // distance to center of bounding sphere
// Check the size of the entity, it's possible that a "too small to see" entity is included in a
// larger octree cell because of its position (for example if it crosses the boundary of a cell it
// pops to the next higher cell. So we want to check to see that the entity is large enough to be seen
// before we consider including it.
float angularSize = frustum.getAngularSize(distance, radius);
if (angularSize > lodScaleFactor * MIN_ENTITY_ANGULAR_DIAMETER &&
frustum.intersects(position, distance, radius)) {
// use the angular size as priority
// we compute the max priority for all frustums
priority = std::max(priority, angularSize);
}
}
return priority;
}
void Perspective::calibrate(Vector* a, Vector* b, Vector* c, Vector* d)
{
//Calculate middle Coords
Vector* center;
center = calcCenter(a, b, c, d);
Vector* va = view->construct(a);
Vector* vb = view->construct(b);
Vector* vc = view->construct(c);
Vector* vd = view->construct(d);
Vector* vz = view->construct(center);
double cosA, cosB, cosC, cosD;
cosA = vz->cos(va);
cosB = vz->cos(vb);
cosC = vz->cos(vc);
cosD = vz->cos(vd);
double sinA, sinB, sinC, sinD;
sinA = sin(acos(cosA));
sinB = sin(acos(cosB));
sinC = sin(-1 * acos(cosC));
sinD = sin(-1 * acos(cosD));
double distA, distB, distC, distD;
distA = -2 * sinC / (cosC*sinA - cosA*sinC);
distC = 2 * sinA / (cosC*sinA - cosA*sinC);
distB = -2 * sinD / (cosD*sinB - cosB*sinD);
distD = 2 * sinB / (cosD*sinB - cosB*sinD);
va->normalize();
vb->normalize();
vc->normalize();
vd->normalize();
va->scale(distA);
vb->scale(distB);
vc->scale(distC);
vd->scale(distD);
Vector* pa = view->sum(va);
Vector* pb = view->sum(vb);
Vector* pc = view->sum(vc);
Vector* pd = view->sum(vd);
//Construct transformation Matrix
Matrix* transform = new Matrix(pa);
//Construct row Vectors
Vector *ax, *ay, *az, *helper;
ax = pa->construct(pd);
helper = pa->construct(pb);
az = ax->cross(helper);
ay = az->cross(ax);
ax->normalize();
ay->normalize();
az->normalize();
//Construct rotation Matrix
Matrix* rotate = new Matrix(ax, ay, az);
Matrix* transRotate = rotate->combine(transform);
Vector* tb = transRotate->multiply(pb);
double displace = -1*tb->v[0]/tb->v[1];
double shearData[][4] = { { 1, displace, 0, 0 },
{ 0, 1, 0, 0 },
{ 0, 0, 1, 0 },
{ 0, 0, 0, 1 } };
Matrix* shear = new Matrix(shearData);
Matrix* rectangify = shear->combine(transRotate);
Vector* rectC = rectangify->multiply(pc);
double resizeData[][4] = { { 1/rectC->v[0], 0, 0, 0 },
{ 0, 1/rectC->v[1], 0, 0 },
{ 0, 0, 1, 0 },
{ 0, 0, 0, 1 } };
Matrix* resize = new Matrix(resizeData);
morph = resize->combine(rectangify);
delete va, vb, vc, vd, vz, pa, pb, pc, pd, ax, ay, az, helper;
delete tb;
delete rectC;
delete transform, rotate, shear, transRotate, rectangify, resize;
}
Point<float> Object::getCenter()
{
calcCenter();
return center;
}
bool poly::giftwrap(){
if (surface.size() != 0)
surface.clear();
// sort by x coordinate
sortPoints(0);
// first points is minimum x
point p1 = points[0]; point* p1_ptr = &points[0];
// 2nd point is found by 2d giftwrap in xy plane
double maxDot = -1.;
point p2; point* p2_ptr;
for (int i=1;i<points.size();i++){
point delta = points[i] - p1;
delta.setX(2,0.);
delta.makeUnit();
double dDot = delta * point(0,1,0);
std::cout << delta << " * " << points[i] << " : " << dDot << std::endl;
if (maxDot < dDot){
maxDot = dDot;
p2 = points[i];
p2_ptr = &points[i];
}
else if (dDot == maxDot)
if ( (points[i] - p1).mag() < p2.mag()){
maxDot = dDot;
p2 = points[i];
p2_ptr = &points[i];
}
}
std::cout << p1 << " " << p2 << std::endl;
// project into plane defined by edge
// between the first two points
point dp = p2 - p1;
dp.makeUnit();
// origin in projected space
point p0 = p1 - dp * (dp*p1);
std::vector<point> pp;
for (int i=1;i<points.size();i++)
if (points[i] != p2)
pp.push_back(points[i] - dp * (dp*points[i]) - p0);
// find all possible pairs of points
std::vector<point> ppairs1;
std::vector<point> ppairs2;
for (int i=0;i<pp.size();i++)
for (int j=i+1;j<pp.size();j++){
ppairs1.push_back(pp[i]);
ppairs2.push_back(pp[j]);
}
pp.clear();
// find the two points in this space
// with the largest opening angle
point pp1, pp2;
double minDot = 2.;
for (int i=0;i<ppairs1.size();i++){
double dDot = (ppairs1[i]*ppairs2[i])/(ppairs1[i].mag()*ppairs2[i].mag());
if (dDot < minDot){
minDot = dDot;
pp1 = ppairs1[i];
pp2 = ppairs2[i];
}
else if (dDot == minDot)
if ( (ppairs1[i]-ppairs2[i]).mag() < (pp1-pp2).mag()){
minDot = dDot;
pp1 = ppairs1[i];
pp2 = ppairs2[i];
}
}
ppairs1.clear();
ppairs2.clear();
// find the poitns in original space
// that match the points at large angles
// these are the first two triangles
for (int i=0;i<points.size();i++){
point mPP = points[i] - dp*(dp*points[i]) - p0;
if (mPP==pp1 || mPP==pp2)
surface.push_back(tri(*p1_ptr,*p2_ptr,points[i]));
}
if (!markEdges())
return false;
// for (int j=0;j<surface.size();j++)
// std::cout << "surface["<<j<< "] : " << surface[j] << std::endl;
// std::cout << std::endl;
while (isSurfaceOpen()){
edge mE;
point mP;
for (int i=0;i<surface.size();i++){
if (!surface[i].getUnconnected(mE, mP))
continue;
dp = mE(1) - mE(0);
dp.makeUnit();
p0 = mE(1) - dp*(dp*mE(1));
point mPP = mP - dp*(dp*mP) - p0;
mPP = -mPP/(mPP.mag());
std::vector<point> ppoints;
for (int j=0;j<points.size();j++){
if (surface[i].hasPoint(points[j]))
continue;
ppoints.push_back(points[j] - dp *(dp*points[j]) - p0);
}
double maxDot = -2.;
point nPP;
// find most colinear
for (int j=0;j<ppoints.size();j++){
double dDot = (ppoints[j]*mPP)/ppoints[j].mag();
if (dDot > maxDot){
maxDot = dDot;
nPP = ppoints[j];
}
// if there are planar points, take the closest one
else if (dDot == maxDot)
if ( ppoints[j].mag() < nPP.mag()){
maxDot = dDot;
nPP = ppoints[j];
}
}
ppoints.clear();
for (int j=0;j<points.size();j++)
if (nPP == points[j] - dp*(dp*points[j]) - p0)
surface.push_back(tri(mE(0),mE(1),points[j]));
// for (int j=0;j<surface.size();j++)
// std::cout << "surface["<<j<< "] : " << surface[j] << std::endl;
// std::cout << std::endl;
if (!markEdges()) {
return false;
//unmarkEdges();
//surface.pop_back();
//for (int j=0;j<surface.size();j++)
//std::cout << "surface["<<j<< "] : " << surface[j] << std::endl;
//std::cout << std::endl;
//continue;
}
break;
}
}
calcCenter();
fixN();
return true;
}
void Effects::setup(int w, int h) {
width = w;
height = h;
cracks.setup(w,h);
// create fbo
glGenFramebuffers(1, &fbo_handle); eglGetError();
glBindFramebuffer(GL_FRAMEBUFFER, fbo_handle);
// create texture.
glGenTextures(1, &fbo_tex); eglGetError();
glActiveTexture(GL_TEXTURE0); eglGetError();
glBindTexture(GL_TEXTURE_2D, fbo_tex); eglGetError();
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL); eglGetError();
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); eglGetError();
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); eglGetError();
glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, fbo_tex, 0); eglGetError();
glGenRenderbuffers(1, &fbo_depth); eglGetError();
// render buffer
glBindRenderbuffer(GL_RENDERBUFFER, fbo_depth); eglGetError();
glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT, w, h);
glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, fbo_depth); eglGetError();
GLenum drawbufs[] = {GL_COLOR_ATTACHMENT0};
glDrawBuffers(1, drawbufs);
glBindFramebuffer(GL_FRAMEBUFFER, 0);
if(!shader.load("effects/effects")) {
printf("Error loading effects shader.\n");
}
vertices[0].setPos(-1, -1, 0);
vertices[1].setPos(1, -1, 0);
vertices[2].setPos(1, 1, 0);
vertices[3].setPos(-1, 1, 0);
float mw = 1;
float mh = 1;
vertices[0].setTex(0, 0);
vertices[1].setTex(mw, 0);
vertices[2].setTex(mw, mh);
vertices[3].setTex(0, mh);
glGenVertexArraysAPPLE(1, &vao); eglGetError();
glBindVertexArrayAPPLE(vao); eglGetError();
GLint pos_attrib = glGetAttribLocation(shader.getProgram(), "pos");
GLint tex_attrib = glGetAttribLocation(shader.getProgram(), "tex");
glEnableVertexAttribArray(pos_attrib);
glEnableVertexAttribArray(tex_attrib);
glGenBuffers(1, &vbo); eglGetError();
glBindBuffer(GL_ARRAY_BUFFER, vbo); eglGetError();
glBufferData(
GL_ARRAY_BUFFER
,sizeof(Vertex) * 4
,vertices[0].pos
,GL_STATIC_DRAW
); eglGetError();
glVertexAttribPointer(
pos_attrib
,3
,GL_FLOAT
,GL_FALSE
,sizeof(Vertex)
,offsetof(Vertex, pos)
);
glVertexAttribPointer(
tex_attrib
,2
,GL_FLOAT
,GL_FALSE
,sizeof(Vertex)
,(GLvoid*)offsetof(Vertex, tex)
);
calcCenter();
unbind();
flip(false);
mirror(false);
crack(false);
}
