static float getDisp(const float *mat,float disp) {
float tmp[4],tmp2[4];
int i;
float alpha;
tmp[0] = _urand();
tmp[1] = _urand();
tmp[2] = _urand();
tmp[3] = _urand();
for (i=0;i<10;i++) {
mulmp4(tmp2,mat,tmp);
alpha = max(absf(tmp2[0]),absf(tmp2[1]));
alpha = max(alpha,absf(tmp2[2]));
alpha = max(alpha,absf(tmp2[3]));
tmp[0] = tmp2[0] / alpha;
tmp[1] = tmp2[1] / alpha;
tmp[2] = tmp2[2] / alpha;
tmp[3] = tmp2[3] / alpha;
}
return alpha;
#undef urand
}
bool func_2(Vector3 vParam0, Vector3 vParam1, float fParam2, int iParam3)
{
if (fParam6 < 0f)
{
fParam6 = 0f;
}
if (!iParam7)
{
if (absf(vParam0.x - vParam3.x) <= fParam6)
{
if (absf(vParam0.y - vParam3.y) <= fParam6)
{
if (absf(vParam0.z - vParam3.z) <= fParam6)
{
return true;
}
}
}
}
else if (absf(vParam0.x - vParam3.x) <= fParam6)
{
if (absf(vParam0.y - vParam3.y) <= fParam6)
{
return true;
}
}
return false;
}
// From "http://devmaster.net/forums/topic/4648-fast-and-accurate-sinecosine/"
float cheriSinf(float x) {
const float B = 4.f/PI;
const float C = -4.f/(PI*PI);
const float P = 0.225f;
float y = B * x + C * x * absf(x);
y = P * (y * absf(y) - y) + y;
return y;
}
GLfloat yValue(GLfloat u,GLfloat v) {
GLfloat Yuv;
if (cos(v) == 0.0 || sin(u) == 0.0) {
return 0;
} else {
Yuv = ((float) cos(v) * (float) powf(absf((float)cos(v)),2.0/SMALL_M - 1.0)) *
((float)sin(u) * (float) powf((float)absf(sin(u)), 2.0/SMALL_N - 1.0));
return Yuv;
}
}
GLfloat nxValue(GLfloat u, GLfloat v){
GLfloat NXuv;
if (cos(v) == 0.0 || cos(u) == 0.0) {
return 0.0;
} else {
NXuv = ((float)cos(v) * (float) powf(absf((float)cos(v)),1.0 - 2.0/SMALL_M)) *
((float)cos(u) * (float) powf(absf((float)cos(u)), 1.0 - 2.0/SMALL_N));
return NXuv;
}
}
Vec2 Phy::calculateNormal(Circle* circle, Box* box, Vec2* close) {
//Calculates normal from closest point of box to centre of ball
//@ Vec2* close only for debug
Vec2 closest = box->position;
float xmax = (box->position.x + box->width / 2);
float xmin = (box->position.x - box->width / 2);
float ymax = (box->position.y + box->height / 2);
float ymin = (box->position.y - box->height / 2);
closest = Vec2(clamp(xmin, xmax, circle->position.x), clamp(ymin, ymax, circle->position.y));
Vec2 normal;
if (closest.equal(circle->position)) {
//ball centre is inside box
//std::cout << "AJAJAJ\n";
Vec2 boxcentre_offset = vecDiff(closest, box->position);
if (absf(boxcentre_offset.x) > absf(boxcentre_offset.y)) {
//clamp to left or right
if (boxcentre_offset.x > 0) {
closest.x = xmax;
}
else {
closest.x = xmin;
}
}
else {
//clamp to top or bottom
if (boxcentre_offset.y > 0) {
closest.y = ymax;
}
else {
closest.y = ymin;
}
}
normal = vecDiff(closest, circle->position);
}
else {
//ball centre is outside box
normal = vecDiff(circle->position, closest);
}
*close = closest;
return normal;
}
GLfloat nyValue(GLfloat u, GLfloat v) {
GLfloat NYuv;
if (cos(v) == 0.0 || sin(u) == 0.0) {
return 0;
} else {
NYuv = ((float)cos(v) * (float) powf(absf((float)cos(v)),1.0 - 2.0/SMALL_M)) *
((float)sin(u) * (float) powf(absf((float)sin(u)), 1.0 - 2.0/SMALL_N));
return NYuv;
}
}
GLfloat xValue(GLfloat u,GLfloat v) {
GLfloat Xuv;
if (cos(v) == 0.0 || cos(u) == 0.0) {
return 0;
} else {
Xuv = ((float)cos(v) * (float) powf(absf((float)cos(v)),2.0/SMALL_M - 1.0)) *
((float) cos(u) * (float) powf( (float) absf(cos(u)), 2.0/SMALL_N - 1.0));
return Xuv;
}
}
/**
* According to "Spatial distribution of daylight CIE standard general sky".
*/
static color get_cie_standard_sky_color(
const vector &ray_dir,
const vector &sun_dir,
const color &zenith_color,
scalar a,
scalar b,
scalar c,
scalar d,
scalar e)
{
eiVector2 ray_pos;
ray_pos.x = acosf(ray_dir.y);
ray_pos.y = atan2f(ray_dir.z, ray_dir.x);
eiVector2 sun_pos;
sun_pos.x = acosf(sun_dir.y);
sun_pos.y = atan2f(sun_dir.z, sun_dir.x);
scalar cos_z_sun = fastercosfull(sun_pos.x);
scalar cos_z = fastercosfull(ray_pos.x);
scalar sin_z_sun = fastersinfull(sun_pos.x);
scalar sin_z = fastersinfull(ray_pos.x);
scalar chi = acosf(cos_z_sun * cos_z + sin_z_sun * sin_z * fastercosfull(absf(ray_pos.y - sun_pos.y)));
scalar cos_chi = fastercosfull(chi);
const scalar pi_over_2 = static_cast<scalar>(eiPI / 2.0f);
scalar f_chi = 1.0f + c * (fastexp(d * chi) - fastexp(d * pi_over_2)) + e * cos_chi * cos_chi;
scalar phi_z = 1.0f + a * fastexp(b / cos_z);
scalar f_z_sun = 1.0f + c * (fastexp(d * sun_pos.x) - fastexp(d * pi_over_2)) + e * cos_z_sun * cos_z_sun;
scalar phi_0 = 1.0f + a * fastexp(b);
scalar ratio = (f_chi * phi_z) / (f_z_sun * phi_0);
return zenith_color * ratio;
}
bool UCamPar::getRadialU2D(const float xu, const float yu,
float * xd, float * yd)
{ // get undistorted image coordinates
int i;
// real (undistorted) pixel position
float rhx = headX / resFactor;
float rhy = headY / resFactor;
// radius
float r;
// real radius for distorted image
float rr = hypot(xu - rhx, yu - rhy);
float x,y;
//
*xd = xu; // initial estimate
*yd = yu;
getRadialD2U(*xd, *yd, &x, &y);
for (i = 0; i < 5; i++)
{ // iterate to clean pixel position
*xd = xu - (x - *xd);
*yd = yu - (y - *yd);
getRadialD2U(*xd, *yd, &x, &y);
// get position in undistorted image
r = hypot(x - rhx, y - rhy);
// stop if error is below 0.05 pixel
if (absf(r - rr) < 0.01)
break;
}
return true;
}
void Phy::resolveCollisionBoundary(ContactBoundary* c) {
Circle* circle = c->circle;
Box* box = c->box;
Vec2 direction;
if (circle->velocity.equal(0, 0)) {
direction = c->normal.normalise().scale(-1);
}
else {
direction = circle->velocity.normalise();
}
Vec2 temp;
while (absf(c->penetration) > 1) {
if (!direction.equal(0, 0)) {
Vec2 error = direction.scale(c->penetration * -1);
circle->position = vecSum(circle->position, error);
}
c->normal = calculateNormal(circle, box, &temp);
c->penetration = circle->radius - sqrt(c->normal.lengthSquared());
}
c->normal = c->normal.normalise();
circle->velocity = vecSum(c->normal.scale(vecDot(c->normal, circle->velocity) * -2), circle->velocity);
}
void Thermocycler::SetPlateControlStrategy() {
if (InControlledRamp())
return;
if (absf(iTargetPlateTemp - GetPlateTemp()) >= PLATE_BANGBANG_THRESHOLD && !InControlledRamp()) {
iPlateControlMode = EBangBang;
iPlatePid.SetMode(MANUAL);
}
else {
iPlateControlMode = EPIDPlate;
iPlatePid.SetMode(AUTOMATIC);
}
if (iRamping) {
if (iTargetPlateTemp >= GetPlateTemp()) {
iDecreasing = false;
if (iTargetPlateTemp < PLATE_PID_INC_LOW_THRESHOLD)
iPlatePid.SetTunings(PLATE_PID_INC_LOW_P, PLATE_PID_INC_LOW_I, PLATE_PID_INC_LOW_D);
else
iPlatePid.SetTunings(PLATE_PID_INC_NORM_P, PLATE_PID_INC_NORM_I, PLATE_PID_INC_NORM_D);
}
else {
iDecreasing = true;
if (iTargetPlateTemp > PLATE_PID_DEC_HIGH_THRESHOLD)
iPlatePid.SetTunings(PLATE_PID_DEC_HIGH_P, PLATE_PID_DEC_HIGH_I, PLATE_PID_DEC_HIGH_D);
else if (iTargetPlateTemp < PLATE_PID_DEC_LOW_THRESHOLD)
iPlatePid.SetTunings(PLATE_PID_DEC_LOW_P, PLATE_PID_DEC_LOW_I, PLATE_PID_DEC_LOW_D);
else
iPlatePid.SetTunings(PLATE_PID_DEC_NORM_P, PLATE_PID_DEC_NORM_I, PLATE_PID_DEC_NORM_D);
}
}
}
void testmipsmath() {
float f1 = -1324.123;
float f2 = 25;
float f3 = 0.01;
writeString("Abs: "); writeDigit((int)absf(f1)); writeString(".\n");
writeString("Sqrt: "); writeDigit((int)sqrtf(f2)); writeString(".\n");
writeString("Recip: "); writeDigit((int)recipf(f3)); writeString(".\n");
writeString("Rsqrt: "); writeDigit((int)rsqrtf(f3)); writeString(".\n");
}
GLfloat zValue(GLfloat u,GLfloat v) {
GLfloat Zuv;
if (sin(v) == 0.0) {
return 0;
} else {
Zuv = ((float)sin(v) * (float) powf(absf((float)sin(v)),2.0/SMALL_M - 1.0));
return Zuv;
}
}
double Math::dectime(double p_value,double p_amount, double p_step) {
float sgn = p_value < 0 ? -1.0 : 1.0;
float val = absf(p_value);
val-=p_amount*p_step;
if (val<0.0)
val=0.0;
return val*sgn;
}
void DrawGLScene()
{
float cameraX = 20.0*cos(cameraRotation);
float cameraZ = 20.0*sin(cameraRotation);
double stretch,height,position,rotation;
glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
glLoadIdentity ();
if(useMyLookat)
myLookAt (cameraX, cameraHeight,cameraZ, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
else
gluLookAt (cameraX, cameraHeight,cameraZ, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
position=cos((double)frame*speed)*2;
height=absf(sin((double)frame*speed)*10)+1;
rotation=cos((double)frame*speed)*90;
if(doTranslate) {
if(height<2) stretch=height/2; else stretch=1;
} else {
stretch=sin((double)frame*speed/3)+1.5;
}
glPushMatrix();
glTranslatef(0,-5,0);
glScalef(8,0.2,8);
drawCube();
glPopMatrix();
glPushMatrix();
glTranslatef(2,-5,0);
glScalef(1.2,-0.5,1.2);
drawCube();
glPopMatrix();
glPushMatrix();
glTranslatef(-2,-5,0);
glScalef(1.2,-0.5,1.2);
drawCube();
glPopMatrix();
if(doTranslate) glTranslatef(position,height-5,0.0);
if(doRotate) glRotatef(rotation,0,1,0);
if(doScale) glScalef(1/sqrt(stretch),stretch,1/sqrt(stretch));
drawCube();
frame++;
cameraRotation += cameraRotationSpeed;
cameraHeight = cameraHeight*0.99 + cameraHeightTarget*0.01;
glutSwapBuffers();
}
GLfloat nzValue(GLfloat u, GLfloat v) {
GLfloat NZuv;
if (sin(v) == 0.0) {
return 0;
} else {
NZuv = ((float)sin(v) * (float) powf((float)absf(sin(v)),1.0 - 2.0/SMALL_M));
return NZuv;
}
}
//TODO max sure glm::length is same as Euclidean norm
//Tangent and binormal vectors of n will be 2nd and 3rd columns of H, respectively
void ShapeUtils::HouseholderOrthogonalization(glm::dvec3 n, glm::mat3 &H) {
//Determine vector h:
double normN = glm::length(n);
glm::dvec3 h;
h.x = absf(n.x - normN) > absf(n.x + normN) ? absf(n.x - normN) : absf(n.x + normN);
h.y = n.y;
h.z = n.z;
//Determine matrix H:
double normH = glm::length(h);
//H = I3 - 2( h*h^t / h^t*h ) -> h times h transpose divided by dot product of h with itself
glm::dmat3 identity;
glm::dmat3 hTimesTranspose = glm::outerProduct(h, h);
double hTransposeTimesH = glm::dot(h, h);
hTimesTranspose = hTimesTranspose / hTransposeTimesH;
hTimesTranspose = hTimesTranspose * 2.0;
H = identity - hTimesTranspose;
}
int Math::decimals(double p_step) {
int max=4;
double llimit = Math::pow(0.1,max);
double ulimit = 1.0-llimit;
int i=0;
while( max) {
float d = absf(p_step) - Math::floor(absf(p_step));
if (d<llimit || d>ulimit)
break;
p_step*=10.0;
max--;
i++;
}
return i;
}
//PreprocessProgram initializes ETA parameters and validates/modifies ramp conditions
void Thermocycler::PreprocessProgram() {
Step* pCurrentStep;
Step* pPreviousStep = NULL;
iProgramHoldDurationS = 0;
iEstimatedTimeRemainingS = 0;
iHasCooled = false;
iProgramControlledRampDurationS = 0;
iProgramFastRampDegrees = 0;
iElapsedFastRampDegrees = 0;
iTotalElapsedFastRampDurationMs = 0;
ipProgram->BeginIteration();
while ((pCurrentStep = ipProgram->GetNextStep()) && !pCurrentStep->IsFinal()) {
//validate ramp
if (pPreviousStep != NULL && pCurrentStep->GetRampDurationS() * 1000 < absf(pCurrentStep->GetTemp() - pPreviousStep->GetTemp()) * PLATE_FAST_RAMP_THRESHOLD_MS) {
//cannot ramp that fast, ignored set ramp
pCurrentStep->SetRampDurationS(0);
}
//update eta hold
iProgramHoldDurationS += pCurrentStep->GetStepDurationS();
//update eta ramp
if (pCurrentStep->GetRampDurationS() > 0) {
//controlled ramp
iProgramControlledRampDurationS += pCurrentStep->GetRampDurationS();
}
else {
//fast ramp
double previousTemp = pPreviousStep ? pPreviousStep->GetTemp() : GetPlateTemp();
iProgramFastRampDegrees += absf(previousTemp - pCurrentStep->GetTemp()) - CYCLE_START_TOLERANCE;
}
pPreviousStep = pCurrentStep;
}
}
void Thermocycler::ControlPeltier() {
Thermocycler::ThermalDirection newDirection = Thermocycler::ThermalDirection::OFF;
if (iProgramState == ERunning || (iProgramState == EComplete && ipCurrentStep != NULL)) {
// Check whether we are nearing target and should switch to PID control
// float plateTemp = GetPlateTemp();
/* Test: Use estimated sample temp instead or measured well temp */
float plateTemp = GetTemp();
if (iPlateControlMode == EBangBang && absf(iTargetPlateTemp - plateTemp) < PLATE_BANGBANG_THRESHOLD) {
iPlateControlMode = EPIDPlate;
iPlatePid.SetMode(AUTOMATIC);
iPlatePid.ResetI();
}
// Apply control mode
if (iPlateControlMode == EBangBang) {
// Full drive
iPeltierPwm = (iTargetPlateTemp > plateTemp) ? MAX_PELTIER_PWM : MIN_PELTIER_PWM;
}
iPlatePid.Compute();
if (iDecreasing && iTargetPlateTemp > PLATE_PID_DEC_LOW_THRESHOLD) {
if (iTargetPlateTemp < plateTemp) {
iPlatePid.ResetI();
}
else {
iDecreasing = false;
}
}
if (iPeltierPwm > 0)
newDirection = HEAT;
else if (iPeltierPwm < 0)
newDirection = COOL;
else
newDirection = OFF;
}
else {
iPeltierPwm = 0;
}
iThermalDirection = newDirection;
SetPeltier(newDirection, iPeltierPwm);
}
int main() {
float a, b, x, y, c, e, Cn = 0, m;
printf("Vvedite a: ");
scanf("%f", &a);
printf("Vvedite b: ");
scanf("%f", &b);
printf("Vvedite e: ");
scanf("%f", &e);
m = 1;
while (m >= e) {
c = GetC(a, b);
m = absf(c - Cn);
Cn = c;
if (f(a)*f(c) < 0) b = c;
if (f(c)*f(b) < 0) a = c;
}
printf("Otvet: %10.4f", c);
getch();
return (0);
}
/**
* @brief Calculate the angle and magnitude of a joystick.
*
* @param js [out] Pointer to a joystick_t structure.
* @param x The raw x-axis value.
* @param y The raw y-axis value.
*/
void calc_joystick_state(struct joystick_t* js, float x, float y) {
float rx, ry, ang;
/*
* Since the joystick center may not be exactly:
* (min + max) / 2
* Then the range from the min to the center and the center to the max
* may be different.
* Because of this, depending on if the current x or y value is greater
* or less than the assoicated axis center value, it needs to be interpolated
* between the center and the minimum or maxmimum rather than between
* the minimum and maximum.
*
* So we have something like this:
* (x min) [-1] ---------*------ [0] (x center) [0] -------- [1] (x max)
* Where the * is the current x value.
* The range is therefore -1 to 1, 0 being the exact center rather than
* the middle of min and max.
*/
if (x == js->center.x)
rx = 0;
else if (x >= js->center.x)
rx = ((float)(x - js->center.x) / (float)(js->max.x - js->center.x));
else
rx = ((float)(x - js->min.x) / (float)(js->center.x - js->min.x)) - 1.0f;
if (y == js->center.y)
ry = 0;
else if (y >= js->center.y)
ry = ((float)(y - js->center.y) / (float)(js->max.y - js->center.y));
else
ry = ((float)(y - js->min.y) / (float)(js->center.y - js->min.y)) - 1.0f;
/* calculate the joystick angle and magnitude */
ang = RAD_TO_DEGREE(atanf(ry / rx));
ang -= 90.0f;
if (rx < 0.0f)
ang -= 180.0f;
js->ang = absf(ang);
js->mag = (float) sqrt((rx * rx) + (ry * ry));
}
void Contourne_Obstacle (int direction)
{
bool obstacleMouvant=0;
do{
float distanceInitiale=capteur.distance.distance1;
THREAD_MSleep(550);//temps d'attente
float variation=capteur.distance.distance1-distanceInitiale;
if (absf(variation)>variationMouvement)
obstacleMouvant=1;
else
obstacleMouvant=0;
}while(obstacleMouvant||capteur.distance.distance1<distanceCritique);
//Si l'obstacle n'est plus là, on continue
if (capteur.distance.distance1>distanceCritique){
MOVE_Resume();
return;
}
//L'obstacle est fixe, on l'évite!
MOVE_Tourner(45);
THREAD_MSleep(550);
if (capteur.distance.distance1>distanceCritique){
MOVE_Avance(20);
MOVE_Tourner(-45);
MOVE_Resume();
return;
}
else{
MOVE_Tourner(-90);
MOVE_Avance(20);
MOVE_Tourner(45);
MOVE_Resume();
return;
}
}
void Thermocycler::ControlPeltier() {
ThermalDirection newDirection = OFF;
if (iProgramState == ERunning || (iProgramState == EComplete && ipCurrentStep != NULL)) {
// Check whether we are nearing target and should switch to PID control
if (iPlateControlMode == EBangBang && absf(iTargetPlateTemp - GetPlateTemp()) < PLATE_BANGBANG_THRESHOLD) {
iPlateControlMode = EPIDPlate;
iPlatePid.SetMode(AUTOMATIC);
iPlatePid.ResetI();
}
// Apply control mode
if (iPlateControlMode == EBangBang)
iPeltierPwm = iTargetPlateTemp > GetPlateTemp() ? MAX_PELTIER_PWM : MIN_PELTIER_PWM;
iPlatePid.Compute();
if (iDecreasing && iTargetPlateTemp > PLATE_PID_DEC_LOW_THRESHOLD) {
if (iTargetPlateTemp < GetPlateTemp())
iPlatePid.ResetI();
else
iDecreasing = false;
}
if (iPeltierPwm > 0)
newDirection = HEAT;
else if (iPeltierPwm < 0)
newDirection = COOL;
else
newDirection = OFF;
}
else {
iPeltierPwm = 0;
}
iThermalDirection = newDirection;
SetPeltier(newDirection, abs(iPeltierPwm));
}
void timestep(graphics_state *state){
int h = state->frame;
/* eye, moves with time */
*((f3*)state->projection_matrix + 3) = (f3) {
0, //sinf(h / 30.0f),
0,
-4}; //-cosf(h / 30.0f)};
/* so does the light */
int i;
for(i = 0; i < state->light_count; i++){
light *light = state->lights + i;
light->location = (f4){
sinf(h / 50.0f + i)*cosf(h / 100.0f + i),
sinf(h/100.0f + i),
cosf(h / 50.0f + i)*cosf(h / 100.0f + i),
0.2f};
}
/* so do the objs */
for(i = 0; i < state->object_count; i++){
sphere *s = state->object_list + i;
s->center.x += s->velocity.x;
s->center.y += s->velocity.y;
s->center.z += s->velocity.z;
/* bounce off the bounding box */
if(s->center.x < state->bounds.top.x)
s->velocity.x = absf(s->velocity.x);
if(s->center.y < state->bounds.top.y)
s->velocity.y = absf(s->velocity.y);
if(s->center.z < state->bounds.top.z)
s->velocity.z = absf(s->velocity.z);
if(s->center.x > state->bounds.bottom.x)
s->velocity.x = -absf(s->velocity.x);
if(s->center.y > state->bounds.bottom.y)
s->velocity.y = -absf(s->velocity.y);
if(s->center.z > state->bounds.bottom.z)
s->velocity.z = -absf(s->velocity.z);
/* bounce off each other */
int j;
for(j = i+1; j < state->object_count; j++){
sphere *s2 = state->object_list + j;
f3 diff = (f3){
s->center.x - s2->center.x,
s->center.y - s2->center.y,
s->center.z - s2->center.z};
float dist = norm3(&diff);
if(dist < s->radius + s2->radius){
normalize3(&diff);
float dot = dot3(&diff, &s->velocity);
if(dot > 0)
/* if they're already flying apart,
* don't bounce them back towards each other */
continue;
float dot2 = dot3(&diff, &s2->velocity);
s->velocity.x -= diff.x*(dot - dot2);
s->velocity.y -= diff.y*(dot - dot2);
s->velocity.z -= diff.z*(dot - dot2);
s2->velocity.x -= diff.x*(dot2 - dot);
s2->velocity.y -= diff.y*(dot2 - dot);
s2->velocity.z -= diff.z*(dot2 - dot);
}
}
}
}
float Plane_Equationf::DistanceTo(const Vec3f &point) const
{
return absf(a * point.x + b * point.y + c * point.z + d) / sqrtf(a * a + b * b + c * c);
}
// --------========--------========--------========--------========--------========--------========
void TweenedLinearCurve::loadFromXmlNode(xmlNodePtr root, float keyDifferenceToIgnore )
{
// look for the curve asset (we only take the first one)
xmlNodePtr child = root->children;
while (child)
{
if (xmlStrcmp( child->name, (const xmlChar*)"PreLoop") == 0)
{
std::string content = XML::parseString(child, "value");
this->setPreLoop( curveLoopTypeFromString(content) );
#if _DEBUG
printf("PreLoop -> got content! [%s] = preLoop[%d]\n", content.c_str(), (int)getPreLoop());
#endif
}
else if (xmlStrcmp( child->name, (const xmlChar*)"PostLoop") == 0)
{
std::string content = XML::parseString(child, "value");
this->setPostLoop( curveLoopTypeFromString(content) );
#ifdef _DEBUG
printf("PostLoop -> got content! [%s] = postLoop[%d]\n", content.c_str(), (int)getPostLoop());
#endif
}
if (xmlStrcmp( child->name, (const xmlChar*)"Keys") == 0)
{
xmlNodePtr key = child->children;
float lastAddedCurveKeyValue=0;
bool didNotAddLastCurveKey=false;
CurveKey lastCurveKey;
EasingFunction lastTweeningFunction;
while(key)
{
if(xmlStrcmp( key->name, (const xmlChar*)"Key") == 0)
{
float position = XML::parseFloat(key, "position");
float value = XML::parseFloat(key, "value");
CurveKey ck(position, value, 0.0f, 0.0f);
EasingFunction tweeningFunction;
if(XML::attrExists(key, "tween"))
{
std::string tween = XML::parseString(key, "tween");
float tweenValue = XML::parseFloat(key, "tweenValue");
tweeningFunction = Tweens::getEasingFunctionForString(tween,tweenValue);
}
else
{
tweeningFunction = &Tweens::linearTween;
}
bool shouldAddCurveKey=true;
if(absf(value-lastAddedCurveKeyValue)<keyDifferenceToIgnore &&
mKeys.size() )
{
shouldAddCurveKey=false;
didNotAddLastCurveKey=true;
lastCurveKey=ck;
lastTweeningFunction=tweeningFunction;
Logger::printf("Walaber", Logger::SV_DEBUG, "Keys -> not adding curvekey! value: [%f] last value: [%f] key difference to ignore: [%f]\n", value, lastAddedCurveKeyValue, keyDifferenceToIgnore);
}
else if(didNotAddLastCurveKey)
{
Logger::printf("Walaber", Logger::SV_DEBUG, "Keys -> got content! position: [%f] value: [%f]\n", lastCurveKey.getPosition(), lastCurveKey.getValue());
addCurveKey(lastCurveKey);
mTweeningFunctions.push_back(lastTweeningFunction);
}
if(shouldAddCurveKey)
{
lastAddedCurveKeyValue=value;
didNotAddLastCurveKey=false;
#ifdef _DEBUG
printf("Keys -> got content! position: [%f] value: [%f]\n", position, value);
#endif
addCurveKey(ck);
mTweeningFunctions.push_back(tweeningFunction);
}
}
key = key->next;
}
if( didNotAddLastCurveKey )
{
addCurveKey( lastCurveKey );
mTweeningFunctions.push_back(lastTweeningFunction);
}
_computeDurations();
computeTangents();
}
child = child->next;
}
}
void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
const math::Vector2f &ground_speed_vector)
{
/* this follows the logic presented in [1] */
float eta;
float xtrack_vel;
float ltrack_vel;
/* get the direction between the last (visited) and next waypoint */
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY());
/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
/* calculate the L1 length required for the desired period */
_L1_distance = _L1_ratio * ground_speed;
/* calculate vector from A to B */
math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
/*
* check if waypoints are on top of each other. If yes,
* skip A and directly continue to B
*/
if (vector_AB.length() < 1.0e-6f) {
vector_AB = get_local_planar_vector(vector_curr_position, vector_B);
}
vector_AB.normalize();
/* calculate the vector from waypoint A to the aircraft */
math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
/* calculate crosstrack error (output only) */
_crosstrack_error = vector_AB % vector_A_to_airplane;
/*
* If the current position is in a +-135 degree angle behind waypoint A
* and further away from A than the L1 distance, then A becomes the L1 point.
* If the aircraft is already between A and B normal L1 logic is applied.
*/
float distance_A_to_airplane = vector_A_to_airplane.length();
float alongTrackDist = vector_A_to_airplane * vector_AB;
/* estimate airplane position WRT to B */
math::Vector2f vector_B_to_airplane_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
float bearing_wp_b = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX());
/* extension from [2], fly directly to A */
if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) {
/* calculate eta to fly to waypoint A */
/* unit vector from waypoint A to current position */
math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
/* velocity across / orthogonal to line */
xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
/* velocity along line */
ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
eta = atan2f(xtrack_vel, ltrack_vel);
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
// XXX this can be useful as last-resort guard, but is currently not needed
#if 0
} else if (absf(bearing_wp_b) > math::radians(80.0f)) {
/* extension, fly back to waypoint */
/* calculate eta to fly to waypoint B */
/* velocity across / orthogonal to line */
xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit);
/* velocity along line */
ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit);
eta = atan2f(xtrack_vel, ltrack_vel);
/* bearing from current position to L1 point */
_nav_bearing = bearing_wp_b;
#endif
} else {
/* calculate eta to fly along the line between A and B */
/* velocity across / orthogonal to line */
xtrack_vel = ground_speed_vector % vector_AB;
/* velocity along line */
ltrack_vel = ground_speed_vector * vector_AB;
/* calculate eta2 (angle of velocity vector relative to line) */
float eta2 = atan2f(xtrack_vel, ltrack_vel);
/* calculate eta1 (angle to L1 point) */
float xtrackErr = vector_A_to_airplane % vector_AB;
float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f);
/* limit output to 45 degrees */
sine_eta1 = math::constrain(sine_eta1, -M_PI_F / 4.0f, +M_PI_F / 4.0f);
float eta1 = asinf(sine_eta1);
eta = eta1 + eta2;
/* bearing from current position to L1 point */
_nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1;
}
/* limit angle to +-90 degrees */
eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
_lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
/* flying to waypoints, not circling them */
_circle_mode = false;
/* the bearing angle, in NED frame */
_bearing_error = eta;
}
// IRQ1 interrupt handler sets keys buffer for this function to read
void handle_key(uint8_t key) {
hit info;
// If the highest bit is not set, this key has not changed
if (!(keys[key] & 0x80)) {
return;
}
bool down = keys[key] & 1;
// Mark key state as read
keys[key] &= 1;
switch (key) {
// View
case KEY_UP:
dPitch += down ? 1.0f : -1.0f;
break;
case KEY_DOWN:
dPitch += down ? -1.0f : 1.0f;
break;
case KEY_LEFT:
dYaw += down ? 1.0f : -1.0f;
break;
case KEY_RIGHT:
dYaw += down ? -1.0f : 1.0f;
break;
case KEY_SPACE:
if (down) {
playerPos.y += 0.1f;
velocity.y += 8.0f;
}
break;
// Check if a block was hit and place a new block next to it
case KEY_Q:
if (!down) {
raytrace(playerPos, ray_dir(160, 100), &info);
if (info.hit) {
int bx = info.x + info.nx;
int by = info.y + info.ny;
int bz = info.z + info.nz;
if (IN_WORLD(bx, by, bz)) {
set_block(bx, by, bz, BLOCK_DIRT);
}
}
}
break;
// Check if a block was hit and remove it
case KEY_E:
if (!down) {
raytrace(playerPos, ray_dir(160, 100), &info);
if (info.hit) {
set_block(info.x, info.y, info.z, BLOCK_AIR);
}
}
break;
case KEY_ESC:
init_world();
break;
}
// Make sure view look speed doesn't add up with low framerates
if (dPitch != 0.0f) dPitch /= absf(dPitch);
if (dYaw != 0.0f) dYaw /= absf(dYaw);
}
