<commit_before>// Copyright 2008 Google Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// This file provides a stub implementation of most plugin entrypoints.

#include <npapi.h>
#include "npn_api.h"
#include "globals_glue.h"

namespace glue {
namespace globals {

void SetLastError(NPP npp, const char *error) {
}

}
}

extern "C" {
  NPError OSCALL NP_GetEntryPoints(NPPluginFuncs *pluginFuncs) {
    pluginFuncs->version = 11;
    pluginFuncs->size = sizeof(*pluginFuncs);
    pluginFuncs->newp = NPP_New;
    pluginFuncs->destroy = NPP_Destroy;
    pluginFuncs->setwindow = NPP_SetWindow;
    pluginFuncs->newstream = NPP_NewStream;
    pluginFuncs->destroystream = NPP_DestroyStream;
    pluginFuncs->asfile = NPP_StreamAsFile;
    pluginFuncs->writeready = NPP_WriteReady;
    pluginFuncs->write = NPP_Write;
    pluginFuncs->print = NPP_Print;
    pluginFuncs->event = NPP_HandleEvent;
    pluginFuncs->urlnotify = NPP_URLNotify;
    pluginFuncs->getvalue = NPP_GetValue;
    pluginFuncs->setvalue = NPP_SetValue;

    return NPERR_NO_ERROR;
  }

#if defined(OS_WINDOWS) || defined(OS_MACOSX)
  NPError OSCALL NP_Initialize(NPNetscapeFuncs *browserFuncs) {
    return InitializeNPNApi(browserFuncs);
  }
#else
  NPError OSCALL NP_Initialize(NPNetscapeFuncs *browserFuncs,
                               NPPluginFuncs *pluginFuncs) {
    NPError retval = InitializeNPNApi(browserFuncs);
    if (retval != NPERR_NO_ERROR) return retval;
    NP_GetEntryPoints(pluginFuncs);
    return NPERR_NO_ERROR;
  }
#endif

  NPError OSCALL NP_Shutdown(void) {
    return NPERR_NO_ERROR;
  }

  NPError NPP_New(NPMIMEType pluginType, NPP instance, uint16_t mode,
                  int16_t argc, char *argn[], char *argv[],
                  NPSavedData *saved) {
    glue::InitializeGlue(instance);
    NPObject *object = glue::CreateStaticNPObject(instance);
    instance->pdata = object;
    return NPERR_NO_ERROR;
  }

  NPError NPP_Destroy(NPP instance, NPSavedData **save) {
    NPObject *object = static_cast<NPObject*>(instance->pdata);
    if (object) {
      NPN_ReleaseObject(object);
      instance->pdata = NULL;
    }
    return NPERR_NO_ERROR;
  }

  NPError NPP_GetValue(NPP instance, NPPVariable variable, void *value) {
    switch (variable) {
      case NPPVpluginScriptableNPObject: {
        void **v = static_cast<void **>(value);
        NPObject *obj = static_cast<NPObject *>(instance->pdata);
        NPN_RetainObject(obj);
        *v = obj;
        break;
      }
      case NPPVpluginNeedsXEmbed:
        *static_cast<NPBool *>(value) = true;
        break;
      default:
        return NPERR_INVALID_PARAM;
        break;
    }
    return NPERR_NO_ERROR;
  }

  NPError NPP_SetValue(NPP instance, NPNVariable variable, void *value) {
    return NPERR_GENERIC_ERROR;
  }

  NPError NPP_SetWindow(NPP instance, NPWindow *window) {
    return NPERR_NO_ERROR;
  }

  void NPP_StreamAsFile(NPP instance, NPStream *stream, const char *fname) {
  }

  int16_t NPP_HandleEvent(NPP instance, void *event) {
    return 0;
  }

  NPError NPP_NewStream(NPP instance, NPMIMEType type, NPStream *stream,
                        NPBool seekable, uint16_t *stype) {
    return NPERR_NO_ERROR;
  }

  NPError NPP_DestroyStream(NPP instance, NPStream *stream, NPReason reason) {
    return NPERR_NO_ERROR;
  }

  int32_t NPP_WriteReady(NPP instance, NPStream *stream) {
    return 0;
  }

  int32_t NPP_Write(NPP instance, NPStream *stream, int32_t offset, int32_t len,
                    void *buffer) {
    return 0;
  }

  void NPP_Print(NPP instance, NPPrint *platformPrint) {
  }

  void NPP_URLNotify(NPP instance, const char *url, NPReason reason,
                     void *notifyData) {
  }
}  // extern "C"
<commit_msg>nixysa/mac: Use CG/Cocoa rendering model<commit_after>// Copyright 2008 Google Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// This file provides a stub implementation of most plugin entrypoints.

#include <npapi.h>
#include "npn_api.h"
#include "globals_glue.h"

namespace glue {
namespace globals {

void SetLastError(NPP npp, const char *error) {
}

}
}

extern "C" {
  NPError OSCALL NP_GetEntryPoints(NPPluginFuncs *pluginFuncs) {
    pluginFuncs->version = 11;
    pluginFuncs->size = sizeof(*pluginFuncs);
    pluginFuncs->newp = NPP_New;
    pluginFuncs->destroy = NPP_Destroy;
    pluginFuncs->setwindow = NPP_SetWindow;
    pluginFuncs->newstream = NPP_NewStream;
    pluginFuncs->destroystream = NPP_DestroyStream;
    pluginFuncs->asfile = NPP_StreamAsFile;
    pluginFuncs->writeready = NPP_WriteReady;
    pluginFuncs->write = NPP_Write;
    pluginFuncs->print = NPP_Print;
    pluginFuncs->event = NPP_HandleEvent;
    pluginFuncs->urlnotify = NPP_URLNotify;
    pluginFuncs->getvalue = NPP_GetValue;
    pluginFuncs->setvalue = NPP_SetValue;

    return NPERR_NO_ERROR;
  }

#if defined(OS_WINDOWS) || defined(OS_MACOSX)
  NPError OSCALL NP_Initialize(NPNetscapeFuncs *browserFuncs) {
    return InitializeNPNApi(browserFuncs);
  }
#else
  NPError OSCALL NP_Initialize(NPNetscapeFuncs *browserFuncs,
                               NPPluginFuncs *pluginFuncs) {
    NPError retval = InitializeNPNApi(browserFuncs);
    if (retval != NPERR_NO_ERROR) return retval;
    NP_GetEntryPoints(pluginFuncs);
    return NPERR_NO_ERROR;
  }
#endif

  NPError OSCALL NP_Shutdown(void) {
    return NPERR_NO_ERROR;
  }

  NPError NPP_New(NPMIMEType pluginType, NPP instance, uint16_t mode,
                  int16_t argc, char *argn[], char *argv[],
                  NPSavedData *saved) {
    glue::InitializeGlue(instance);
    NPObject *object = glue::CreateStaticNPObject(instance);
    instance->pdata = object;

#if defined(OS_MACOSX)
    // On the Mac, starting from Google Chrome 22, we need to negotiate
    // CoreGraphics and Cocoa even though we don't use it.
    // See: http://code.google.com/p/chromium/issues/detail?id=134761
    // Code ported from http://gwt-code-reviews.appspot.com/1844803/

    // Select the right drawing model if necessary
    NPBool supports_core_graphics = false;
    if (NPN_GetValue(instance, NPNVsupportsCoreGraphicsBool,
                     &supports_core_graphics) == NPERR_NO_ERROR &&
        supports_core_graphics) {
      NPN_SetValue(instance, NPPVpluginDrawingModel,
                   reinterpret_cast<void*>(NPDrawingModelCoreGraphics));
    }
    // Select the Cocoa event model
    NPBool supports_cocoa_events = false;
    if (NPN_GetValue(instance, NPNVsupportsCocoaBool,
                     &supports_cocoa_events) == NPERR_NO_ERROR &&
        supports_cocoa_events) {
      NPN_SetValue(instance, NPPVpluginEventModel,
                   reinterpret_cast<void*>(NPEventModelCocoa));
    }
#endif
    return NPERR_NO_ERROR;
  }

  NPError NPP_Destroy(NPP instance, NPSavedData **save) {
    NPObject *object = static_cast<NPObject*>(instance->pdata);
    if (object) {
      NPN_ReleaseObject(object);
      instance->pdata = NULL;
    }
    return NPERR_NO_ERROR;
  }

  NPError NPP_GetValue(NPP instance, NPPVariable variable, void *value) {
    switch (variable) {
      case NPPVpluginScriptableNPObject: {
        void **v = static_cast<void **>(value);
        NPObject *obj = static_cast<NPObject *>(instance->pdata);
        NPN_RetainObject(obj);
        *v = obj;
        break;
      }
      case NPPVpluginNeedsXEmbed:
        *static_cast<NPBool *>(value) = true;
        break;
      default:
        return NPERR_INVALID_PARAM;
        break;
    }
    return NPERR_NO_ERROR;
  }

  NPError NPP_SetValue(NPP instance, NPNVariable variable, void *value) {
    return NPERR_GENERIC_ERROR;
  }

  NPError NPP_SetWindow(NPP instance, NPWindow *window) {
    return NPERR_NO_ERROR;
  }

  void NPP_StreamAsFile(NPP instance, NPStream *stream, const char *fname) {
  }

  int16_t NPP_HandleEvent(NPP instance, void *event) {
    return 0;
  }

  NPError NPP_NewStream(NPP instance, NPMIMEType type, NPStream *stream,
                        NPBool seekable, uint16_t *stype) {
    return NPERR_NO_ERROR;
  }

  NPError NPP_DestroyStream(NPP instance, NPStream *stream, NPReason reason) {
    return NPERR_NO_ERROR;
  }

  int32_t NPP_WriteReady(NPP instance, NPStream *stream) {
    return 0;
  }

  int32_t NPP_Write(NPP instance, NPStream *stream, int32_t offset, int32_t len,
                    void *buffer) {
    return 0;
  }

  void NPP_Print(NPP instance, NPPrint *platformPrint) {
  }

  void NPP_URLNotify(NPP instance, const char *url, NPReason reason,
                     void *notifyData) {
  }
}  // extern "C"
<|endoftext|>


<commit_before>#include "MotionControl.hpp"
#include <SystemState.hpp>
#include <RobotConfig.hpp>
#include <Robot.hpp>
#include <Utils.hpp>
#include "TrapezoidalMotion.hpp"
#include <Geometry2d/Util.hpp>
#include <planning/MotionInstant.hpp>

#include <cmath>
#include <stdio.h>
#include <algorithm>

using namespace std;
using namespace Geometry2d;
using namespace Planning;

#pragma mark Config Variables

REGISTER_CONFIGURABLE(MotionControl);

ConfigDouble* MotionControl::_max_acceleration;
ConfigDouble* MotionControl::_max_velocity;

void MotionControl::createConfiguration(Configuration* cfg) {
    _max_acceleration =
        new ConfigDouble(cfg, "MotionControl/Max Acceleration", 1.5);
    _max_velocity = new ConfigDouble(cfg, "MotionControl/Max Velocity", 2.0);
}

#pragma mark MotionControl

MotionControl::MotionControl(OurRobot* robot) : _angleController(0, 0, 0, 50) {
    _robot = robot;

    _robot->radioTx.set_robot_id(_robot->shell());
    _lastCmdTime = -1;
}

void MotionControl::run() {
    if (!_robot) return;

    const MotionConstraints& constraints = _robot->motionConstraints();

    // update PID parameters
    _positionXController.kp = *_robot->config->translation.p;
    _positionXController.ki = *_robot->config->translation.i;
    _positionXController.setWindup(*_robot->config->translation.i_windup);
    _positionXController.kd = *_robot->config->translation.d;
    _positionYController.kp = *_robot->config->translation.p;
    _positionYController.ki = *_robot->config->translation.i;
    _positionYController.setWindup(*_robot->config->translation.i_windup);
    _positionYController.kd = *_robot->config->translation.d;
    _angleController.kp = *_robot->config->rotation.p;
    _angleController.ki = *_robot->config->rotation.i;
    _angleController.kd = *_robot->config->rotation.d;

    float timeIntoPath =
            ((float)(RJ::timestamp() - _robot->path().startTime())) *
            TimestampToSecs +
            1.0 / 60.0;

    // evaluate path - where should we be right now?
    boost::optional<RobotInstant> optTarget =
            _robot->path().evaluate(timeIntoPath);

    // Angle control //////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////

    float targetW = 0;
    auto& rotationCommand = _robot->rotationCommand();
    const auto& rotationConstraints = _robot->rotationConstraints();

    boost::optional<Geometry2d::Point> targetPt;
    const auto& motionCommand = _robot->motionCommand();
    if (motionCommand->getCommandType() == MotionCommand::Pivot) {
        PivotCommand command = *static_cast<PivotCommand*>(motionCommand.get());
        targetPt = command.pivotTarget;
    }

/*
    switch (rotationCommand.getCommandType()) {
        case RotationCommand::FacePoint:
            targetPt = static_cast<const Planning::FacePointCommand&>(rotationCommand).targetPos;
            break;
        case RotationCommand::None:
            // do nothing
            break;
        default:
            debugThrow("RotationCommand Not implemented");
            break;
    }
    */
    float targetAngleFinal = 0;
    if (optTarget) {
        std::cout<<"optTarget"<<std::endl;
        if (optTarget->angle) {

            std::cout<<"optTarget->angle"<<std::endl;
            if (optTarget->angle->angle) {

                std::cout<<"optTarget->final"<<std::endl;
                targetAngleFinal = *optTarget->angle->angle;
                std::cout<<targetAngleFinal<<std::endl;
            }
        }
    }
    if (targetPt) {
        // fixing the angle ensures that we don't go the long way around to get
        // to our final angle
        std::cout<<"targetPt"<<std::endl;
        targetAngleFinal = (*targetPt - _robot->pos).angle();
    }

    float angleError = fixAngleRadians(targetAngleFinal - _robot->angle);

    std::cout<<"angleError:"<< angleError<<std::endl;
    targetW = _angleController.run(angleError);

    // limit W
    if (abs(targetW) > (rotationConstraints.maxSpeed)) {
        if (targetW > 0) {
            targetW = (rotationConstraints.maxSpeed);
        } else {
            targetW = -(rotationConstraints.maxSpeed);
        }
    }

        /*
        _robot->addText(QString("targetW: %1").arg(targetW));
        _robot->addText(QString("angleError: %1").arg(angleError));
        _robot->addText(QString("targetGlobalAngle: %1").arg(targetAngleFinal));
        _robot->addText(QString("angle: %1").arg(_robot->angle));
        */
    _targetAngleVel(targetW);
    //_targetAngleVel(targetW);

    // handle body velocity for pivot command
    if (motionCommand->getCommandType() == MotionCommand::Pivot) {
        float r = Robot_Radius;
        const float FudgeFactor = *_robot->config->pivotVelMultiplier;
        float speed = RadiansToDegrees(r * targetW * FudgeFactor);
        Point vel(speed, 0);

        // the robot body coordinate system is wierd...
        vel.rotate(-M_PI_2);

        _targetBodyVel(vel);

        return;  // pivot handles both angle and position
    }

    // Position control ///////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////

    MotionInstant target;
    // if no target position is given, we don't have a path to follow
//    if (!_robot->path()) {
//        _targetBodyVel(Point(0, 0));
//        std::cout<<"No Path"<<std::endl;
//        debugThrow("No Path set");
//        return;
//    } else {
        //
        // Path following
        //

        // convert from microseconds to seconds

    if (!optTarget) {
        // use the path end if our timeIntoPath is greater than the duration
        target.vel = Point();
        target.pos = _robot->path().end().motion.pos;
    } else {
        target = optTarget->motion;
    }
    // tracking error
    Point posError = target.pos - _robot->pos;

    // acceleration factor
    Point acceleration;
    boost::optional<RobotInstant> nextTarget =
        _robot->path().evaluate(timeIntoPath + 1.0 / 60.0);
    if (nextTarget) {
        acceleration = (nextTarget->motion.vel - target.vel) / 60.0f;
    } else {
        acceleration = {0, 0};
    }
    Point accelFactor =
        acceleration * 60.0f * (*_robot->config->accelerationMultiplier);

    target.vel += accelFactor;

    // PID on position
    target.vel.x += _positionXController.run(posError.x);
    target.vel.y += _positionYController.run(posError.y);

    // draw target pt
    _robot->state()->drawCircle(target.pos, .04, Qt::red, "MotionControl");
    _robot->state()->drawLine(target.pos, target.pos + target.vel, Qt::blue,
                              "MotionControl");

    // convert from world to body coordinates
    target.vel = target.vel.rotated(-_robot->angle);
//    }

    this->_targetBodyVel(target.vel);
}

void MotionControl::stopped() {
    _targetBodyVel(Point(0, 0));
    _targetAngleVel(0);
}

void MotionControl::_targetAngleVel(float angleVel) {
    // velocity multiplier
    angleVel *= *_robot->config->angleVelMultiplier;

    // convert units
    angleVel = RadiansToDegrees(angleVel);

    // If the angular speed is very low, it won't make the robot move at all, so
    // we make sure it's above a threshold value
    float minEffectiveAngularSpeed = *_robot->config->minEffectiveAngularSpeed;
    if (std::abs(angleVel) < minEffectiveAngularSpeed &&
        std::abs(angleVel) > 0.2) {
        angleVel =
            angleVel > 0 ? minEffectiveAngularSpeed : -minEffectiveAngularSpeed;
    }

    // the robot firmware still speaks degrees, so that's how we send it over
    _robot->radioTx.set_body_w(angleVel);
}

void MotionControl::_targetBodyVel(Point targetVel) {
    // Limit Velocity
    targetVel.clamp(*_max_velocity);

    // Limit Acceleration
    if (_lastCmdTime == -1) {
        targetVel.clamp(*_max_acceleration);
    } else {
        float dt = (float)((RJ::timestamp() - _lastCmdTime) / 1000000.0f);
        Point targetAccel = (targetVel - _lastVelCmd) / dt;
        targetAccel.clamp(*_max_acceleration);

        targetVel = _lastVelCmd + targetAccel * dt;
    }

    // make sure we don't send any bad values
    if (isnan(targetVel.x) || isnan(targetVel.y)) {
        targetVel = Point(0, 0);
    }

    // track these values so we can limit acceleration
    _lastVelCmd = targetVel;
    _lastCmdTime = RJ::timestamp();

    // velocity multiplier
    targetVel *= *_robot->config->velMultiplier;

    // if the velocity is nonzero, make sure it's not so small that the robot
    // doesn't even move
    float minEffectiveVelocity = *_robot->config->minEffectiveVelocity;
    if (targetVel.mag() < minEffectiveVelocity && targetVel.mag() > 0.05) {
        targetVel = targetVel.normalized() * minEffectiveVelocity;
    }

    // set radioTx values
    _robot->radioTx.set_body_x(targetVel.x);
    _robot->radioTx.set_body_y(targetVel.y);
}
<commit_msg>Change it so that the Motion Planner uses the End Motion and direction if the path is finished.<commit_after>#include "MotionControl.hpp"
#include <SystemState.hpp>
#include <RobotConfig.hpp>
#include <Robot.hpp>
#include <Utils.hpp>
#include "TrapezoidalMotion.hpp"
#include <Geometry2d/Util.hpp>
#include <planning/MotionInstant.hpp>

#include <cmath>
#include <stdio.h>
#include <algorithm>

using namespace std;
using namespace Geometry2d;
using namespace Planning;

#pragma mark Config Variables

REGISTER_CONFIGURABLE(MotionControl);

ConfigDouble* MotionControl::_max_acceleration;
ConfigDouble* MotionControl::_max_velocity;

void MotionControl::createConfiguration(Configuration* cfg) {
    _max_acceleration =
        new ConfigDouble(cfg, "MotionControl/Max Acceleration", 1.5);
    _max_velocity = new ConfigDouble(cfg, "MotionControl/Max Velocity", 2.0);
}

#pragma mark MotionControl

MotionControl::MotionControl(OurRobot* robot) : _angleController(0, 0, 0, 50) {
    _robot = robot;

    _robot->radioTx.set_robot_id(_robot->shell());
    _lastCmdTime = -1;
}

void MotionControl::run() {
    if (!_robot) return;

    const MotionConstraints& constraints = _robot->motionConstraints();

    // update PID parameters
    _positionXController.kp = *_robot->config->translation.p;
    _positionXController.ki = *_robot->config->translation.i;
    _positionXController.setWindup(*_robot->config->translation.i_windup);
    _positionXController.kd = *_robot->config->translation.d;
    _positionYController.kp = *_robot->config->translation.p;
    _positionYController.ki = *_robot->config->translation.i;
    _positionYController.setWindup(*_robot->config->translation.i_windup);
    _positionYController.kd = *_robot->config->translation.d;
    _angleController.kp = *_robot->config->rotation.p;
    _angleController.ki = *_robot->config->rotation.i;
    _angleController.kd = *_robot->config->rotation.d;

    float timeIntoPath =
            ((float)(RJ::timestamp() - _robot->path().startTime())) *
            TimestampToSecs +
            1.0 / 60.0;

    // evaluate path - where should we be right now?
    boost::optional<RobotInstant> optTarget =
            _robot->path().evaluate(timeIntoPath);
    if (!optTarget) {
        optTarget = _robot->path().end();
    }

    // Angle control //////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////

    float targetW = 0;
    auto& rotationCommand = _robot->rotationCommand();
    const auto& rotationConstraints = _robot->rotationConstraints();

    boost::optional<Geometry2d::Point> targetPt;
    const auto& motionCommand = _robot->motionCommand();

    float targetAngleFinal = 0;
    if (motionCommand->getCommandType() == MotionCommand::Pivot) {
        PivotCommand command = *static_cast<PivotCommand*>(motionCommand.get());
        targetPt = command.pivotTarget;
    } else {
        if (optTarget) {
            std::cout << "optTarget" << std::endl;
            if (optTarget->angle) {

                std::cout << "optTarget->angle" << std::endl;
                if (optTarget->angle->angle) {

                    std::cout << "optTarget->final" << std::endl;
                    targetAngleFinal = *optTarget->angle->angle;
                    std::cout << targetAngleFinal << std::endl;
                }
            }
        }
    }
    if (targetPt) {
        // fixing the angle ensures that we don't go the long way around to get
        // to our final angle
        std::cout<<"targetPt"<<std::endl;
        targetAngleFinal = (*targetPt - _robot->pos).angle();
    }

    float angleError = fixAngleRadians(targetAngleFinal - _robot->angle);

    std::cout<<"angleError:"<< angleError<<std::endl;
    targetW = _angleController.run(angleError);

    // limit W
    if (abs(targetW) > (rotationConstraints.maxSpeed)) {
        if (targetW > 0) {
            targetW = (rotationConstraints.maxSpeed);
        } else {
            targetW = -(rotationConstraints.maxSpeed);
        }
    }

        /*
        _robot->addText(QString("targetW: %1").arg(targetW));
        _robot->addText(QString("angleError: %1").arg(angleError));
        _robot->addText(QString("targetGlobalAngle: %1").arg(targetAngleFinal));
        _robot->addText(QString("angle: %1").arg(_robot->angle));
        */
    _targetAngleVel(targetW);
    //_targetAngleVel(targetW);

    // handle body velocity for pivot command
    if (motionCommand->getCommandType() == MotionCommand::Pivot) {
        float r = Robot_Radius;
        const float FudgeFactor = *_robot->config->pivotVelMultiplier;
        float speed = RadiansToDegrees(r * targetW * FudgeFactor);
        Point vel(speed, 0);

        // the robot body coordinate system is wierd...
        vel.rotate(-M_PI_2);

        _targetBodyVel(vel);

        return;  // pivot handles both angle and position
    }

    // Position control ///////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////

    MotionInstant target;
    // if no target position is given, we don't have a path to follow
//    if (!_robot->path()) {
//        _targetBodyVel(Point(0, 0));
//        std::cout<<"No Path"<<std::endl;
//        debugThrow("No Path set");
//        return;
//    } else {
        //
        // Path following
        //

        // convert from microseconds to seconds

    if (!optTarget) {
        // use the path end if our timeIntoPath is greater than the duration
        target.vel = Point();
        target.pos = _robot->path().end().motion.pos;
    } else {
        target = optTarget->motion;
    }
    // tracking error
    Point posError = target.pos - _robot->pos;

    // acceleration factor
    Point acceleration;
    boost::optional<RobotInstant> nextTarget =
        _robot->path().evaluate(timeIntoPath + 1.0 / 60.0);
    if (nextTarget) {
        acceleration = (nextTarget->motion.vel - target.vel) / 60.0f;
    } else {
        acceleration = {0, 0};
    }
    Point accelFactor =
        acceleration * 60.0f * (*_robot->config->accelerationMultiplier);

    target.vel += accelFactor;

    // PID on position
    target.vel.x += _positionXController.run(posError.x);
    target.vel.y += _positionYController.run(posError.y);

    // draw target pt
    _robot->state()->drawCircle(target.pos, .04, Qt::red, "MotionControl");
    _robot->state()->drawLine(target.pos, target.pos + target.vel, Qt::blue,
                              "MotionControl");

    // convert from world to body coordinates
    target.vel = target.vel.rotated(-_robot->angle);
//    }

    this->_targetBodyVel(target.vel);
}

void MotionControl::stopped() {
    _targetBodyVel(Point(0, 0));
    _targetAngleVel(0);
}

void MotionControl::_targetAngleVel(float angleVel) {
    // velocity multiplier
    angleVel *= *_robot->config->angleVelMultiplier;

    // convert units
    angleVel = RadiansToDegrees(angleVel);

    // If the angular speed is very low, it won't make the robot move at all, so
    // we make sure it's above a threshold value
    float minEffectiveAngularSpeed = *_robot->config->minEffectiveAngularSpeed;
    if (std::abs(angleVel) < minEffectiveAngularSpeed &&
        std::abs(angleVel) > 0.2) {
        angleVel =
            angleVel > 0 ? minEffectiveAngularSpeed : -minEffectiveAngularSpeed;
    }

    // the robot firmware still speaks degrees, so that's how we send it over
    _robot->radioTx.set_body_w(angleVel);
}

void MotionControl::_targetBodyVel(Point targetVel) {
    // Limit Velocity
    targetVel.clamp(*_max_velocity);

    // Limit Acceleration
    if (_lastCmdTime == -1) {
        targetVel.clamp(*_max_acceleration);
    } else {
        float dt = (float)((RJ::timestamp() - _lastCmdTime) / 1000000.0f);
        Point targetAccel = (targetVel - _lastVelCmd) / dt;
        targetAccel.clamp(*_max_acceleration);

        targetVel = _lastVelCmd + targetAccel * dt;
    }

    // make sure we don't send any bad values
    if (isnan(targetVel.x) || isnan(targetVel.y)) {
        targetVel = Point(0, 0);
    }

    // track these values so we can limit acceleration
    _lastVelCmd = targetVel;
    _lastCmdTime = RJ::timestamp();

    // velocity multiplier
    targetVel *= *_robot->config->velMultiplier;

    // if the velocity is nonzero, make sure it's not so small that the robot
    // doesn't even move
    float minEffectiveVelocity = *_robot->config->minEffectiveVelocity;
    if (targetVel.mag() < minEffectiveVelocity && targetVel.mag() > 0.05) {
        targetVel = targetVel.normalized() * minEffectiveVelocity;
    }

    // set radioTx values
    _robot->radioTx.set_body_x(targetVel.x);
    _robot->radioTx.set_body_y(targetVel.y);
}
<|endoftext|>


<commit_before>// $Id: snapShot.C,v 1.21 2001/03/11 19:32:31 anker Exp $

// BALL includes
#include <BALL/MOLMEC/COMMON/snapShot.h>
#include <BALL/CONCEPT/XDRPersistenceManager.h>

// STL includes
#include <algorithm>

using namespace std;

namespace BALL
{

	// The default constructor of the SnapShot class.
  SnapShot::SnapShot()
		throw()
		:	index_(0),
			number_of_atoms_(0),
			potential_energy_(0.0),
			kinetic_energy_(0.0),
			atom_positions_(0),
			atom_velocities_(0),
			atom_forces_(0),
			valid_(false)
	{
	}

	// The copy constructor of the SnapShot class.
	SnapShot::SnapShot (const SnapShot& snapshot)
		throw()
		:	index_(snapshot.index_),
			number_of_atoms_(snapshot.number_of_atoms_),
			potential_energy_(snapshot.potential_energy_),
			kinetic_energy_(snapshot.kinetic_energy_),
			atom_positions_(snapshot.atom_positions_),
			atom_velocities_(snapshot.atom_velocities_),
			atom_forces_(snapshot.atom_forces_),
			valid_(snapshot.valid_)
	{
	}

	// The destructor just deletes all STL components (superfluous and just
	// for clarity)
	SnapShot::~SnapShot()
		throw()
	{
		clear();

		valid_ = false;
	}

	// The assignment operator
	const SnapShot& SnapShot::operator = (const SnapShot& snapshot)
		throw()
	{
		valid_ = snapshot.valid_;
		index_ = snapshot.index_;
		number_of_atoms_ = snapshot.number_of_atoms_;
		potential_energy_ = snapshot.potential_energy_;
		kinetic_energy_ = snapshot.kinetic_energy_;
		atom_positions_ = snapshot.atom_positions_;
		atom_velocities_ = snapshot.atom_velocities_;
		atom_forces_ = snapshot.atom_forces_;

		return *this;
	}

	void SnapShot::clear()
		throw()
	{
		valid_ = false;
		index_ = 0;
		number_of_atoms_ = 0;
		potential_energy_ = 0.0;
		kinetic_energy_ = 0.0;

		atom_positions_.clear();
		atom_velocities_.clear();
		atom_forces_.clear();
	}

	bool SnapShot::operator == (const SnapShot& snapshot) const
		throw()
	{
		return ((valid_ == snapshot.valid_)
			&& (index_ == snapshot.index_)
			&& (number_of_atoms_ == snapshot.number_of_atoms_)
			&& (potential_energy_ == snapshot.potential_energy_)
			&& (kinetic_energy_ == snapshot.kinetic_energy_)
			&& (atom_positions_ == snapshot.atom_positions_)
			&& (atom_velocities_ == snapshot.atom_velocities_)
			&& (atom_forces_ == snapshot.atom_forces_));
	}

	bool SnapShot::isValid() const
		throw()
	{
		return valid_;
	}

	void SnapShot::setIndex(Size index)
		throw()
	{
		index_ = index;
	}

	Size SnapShot::getIndex() const
		throw()
	{
		return index_;
	}

	void SnapShot::setNumberOfAtoms(Size number_of_atoms)
		throw()
	{
		number_of_atoms_ = number_of_atoms;
	}

	Size SnapShot::getNumberOfAtoms() const
		throw()
	{
		return number_of_atoms_;
	}

	DoubleReal SnapShot::getPotentialEnergy() const
		throw()
	{
		return potential_energy_;
	}

	void SnapShot::setPotentialEnergy(DoubleReal potential_energy)
		throw()
	{
		potential_energy_ = potential_energy;
	}

	DoubleReal SnapShot::getKineticEnergy() const
		throw()
	{
		return kinetic_energy_;
	}

	void SnapShot::setKineticEnergy(DoubleReal kinetic_energy)
		throw()
	{
		kinetic_energy_ = kinetic_energy;
	}

	void SnapShot::setAtomPositions(const ::std::vector<Vector3>& atom_postions)
		throw()
	{
		atom_positions_ = atom_postions;
	}

	const ::std::vector<Vector3>& SnapShot::getAtomPositions() const
		throw()
	{
		return atom_positions_;
	}

	void SnapShot::setAtomVelocitites(const ::std::vector<Vector3>& atom_velocities)
		throw()
	{
		atom_velocities_ = atom_velocities;
	}

	const ::std::vector<Vector3>& SnapShot::getAtomVelocities() const
		throw()
	{
		return atom_velocities_;
	}

	void SnapShot::setAtomForces(const ::std::vector<Vector3>& atom_forces)
		throw()
	{
		atom_forces_ = atom_forces;
	}

	const ::std::vector<Vector3>& SnapShot::getAtomForces() const
		throw()
	{
		return atom_forces_;
	}

	void SnapShot::takeSnapShot(const System& system)
		throw(Exception::OutOfMemory)
	{
		number_of_atoms_ = system.countAtoms();

		AtomIterator atom_it = system.beginAtom();

		// reserve memory
		atom_positions_.resize(number_of_atoms_);
		atom_velocities_.resize(number_of_atoms_);
		atom_forces_.resize(number_of_atoms_);

		// This is the data section of the snapshot object
		for (Size i = 0; +atom_it; ++atom_it, ++i)
		{
			atom_positions_[i] = atom_it->getPosition();
			atom_velocities_[i] = atom_it->getVelocity();
			atom_forces_[i] = atom_it->getForce();
		}
	}

	void SnapShot::applySnapShot(System& system) const
		throw()
	{
		if (system.countAtoms() != number_of_atoms_)
		{
			Log.error () << "SnapShot::applySnapShot(): "
				<< "Atom counts do not match." << endl;
			return;
		}

		AtomIterator atom_it = system.beginAtom();

		for (Size i = 0; +atom_it; ++atom_it, ++i)
		{
			atom_it->setPosition(atom_positions_[i]);
			atom_it->setVelocity(atom_velocities_[i]);
			atom_it->setForce(atom_forces_[i]);
		}
	}

	void SnapShot::getAtomPositions(const System& system)
		throw(Exception::OutOfMemory)
	{
		// obtain the number of atoms
		number_of_atoms_ = system.countAtoms();

		// reserve memory
		atom_positions_.resize(number_of_atoms_);

		// copy data
		AtomIterator atom_it = system.beginAtom();
		for (Size i = 0; +atom_it; ++atom_it, ++i)
		{
			atom_positions_[i] = atom_it->getPosition();
		}
	}

	void SnapShot::setAtomPositions(System& system) const
		throw()
	{
		if (system.countAtoms() != number_of_atoms_)
		{
			Log.error () << "SnapShot::applySnapShot(): "
				<< "Atom counts do not match." << endl;
			return;
		}

		AtomIterator atom_it = system.beginAtom();

		for (Size i = 0; +atom_it; ++atom_it, ++i)
		{
			atom_it->setPosition(atom_positions_[i]);
		}
	}

	void SnapShot::getAtomVelocities(const System& system)
		throw(Exception::OutOfMemory)
	{
		number_of_atoms_ = system.countAtoms();
		atom_velocities_.resize(number_of_atoms_);
		AtomIterator atom_it = system.beginAtom();
		for (Size i = 0; +atom_it; ++atom_it, ++i)
		{
			atom_velocities_[i] = atom_it->getVelocity();
		}
	}

	void SnapShot::setAtomVelocitites(System& system) const
		throw()
	{
		if (system.countAtoms() != number_of_atoms_)
		{
			Log.error () << "SnapShot::applySnapShot(): "
				<< "Atom counts do not match." << endl;
			return;
		}

		AtomIterator atom_it = system.beginAtom();

		// This is the data section of the snapshot object
		for (Size i = 0; +atom_it; ++atom_it, ++i)
		{
			atom_it->setVelocity(atom_velocities_[i]);
		}
	}

	void SnapShot::getAtomForces(const System& system)
		throw(Exception::OutOfMemory)
	{
		number_of_atoms_ = system.countAtoms();
		atom_forces_.resize(number_of_atoms_);
		AtomIterator atom_it = system.beginAtom();
		for (Size i = 0; +atom_it; ++atom_it, ++i)
		{
			atom_forces_[i] = atom_it->getForce();
		}
	}

	void SnapShot::setAtomForces(System& system) const
		throw()
	{
		if (system.countAtoms() != number_of_atoms_)
		{
			Log.error () << "SnapShot::applySnapShot(): "
				<< "Atom counts do not match." << endl;
			return;
		}

		AtomIterator atom_it = system.beginAtom();

		// This is the data section of the snapshot object
		for (Size i = 0; +atom_it; ++atom_it, ++i)
		{
			atom_it->setForce(atom_forces_[i]);
		}
	}

	::std::ostream& operator << (::std::ostream& os, const SnapShot& ss)
	{
		os << ss.getNumberOfAtoms() << endl;

		vector<Vector3> data = ss.getAtomPositions();

		vector<Vector3>::const_iterator it = data.begin();
		for (; it != data.end(); ++it)
		{
			os << *it << endl;
		}

		data = ss.getAtomVelocities();
		for (it = data.begin(); it != data.end(); ++it)
		{
			os << *it << endl;
		}

		data = ss.getAtomForces();
		for (it = data.begin(); it != data.end(); ++it)
		{
			os << *it << endl;
		}
		return os;
	}

	::std::istream& operator >> (::std::istream& is, SnapShot& ss)
	{
		// BAUSTELLE:
		// this istream does not check anything!

		Size number;
		is >> number;

		vector<Vector3> data;
		data.resize(number);

		for (Size i = 0; i < number; ++i)
		{
			is >> data[i];
		}
		ss.setAtomPositions(data);

		for (Size i = 0; i < number; ++i)
		{
			is >> data[i];
		}
		ss.setAtomVelocitites(data);

		for (Size i = 0; i < number; ++i)
		{
			is >> data[i];
		}
		ss.setAtomForces(data);
		return is;
	}

}	// end of namespace BALL
<commit_msg>fixed: no sigsegv anymore because of accessing non-existant atom data fixed: improved diagnostic output in error cases<commit_after>// $Id: snapShot.C,v 1.22 2001/03/21 16:23:33 anker Exp $

// BALL includes
#include <BALL/MOLMEC/COMMON/snapShot.h>
#include <BALL/CONCEPT/XDRPersistenceManager.h>

// STL includes
#include <algorithm>

using namespace std;

namespace BALL
{

	// The default constructor of the SnapShot class.
  SnapShot::SnapShot()
		throw()
		:	index_(0),
			number_of_atoms_(0),
			potential_energy_(0.0),
			kinetic_energy_(0.0),
			atom_positions_(0),
			atom_velocities_(0),
			atom_forces_(0),
			valid_(false)
	{
	}

	// The copy constructor of the SnapShot class.
	SnapShot::SnapShot (const SnapShot& snapshot)
		throw()
		:	index_(snapshot.index_),
			number_of_atoms_(snapshot.number_of_atoms_),
			potential_energy_(snapshot.potential_energy_),
			kinetic_energy_(snapshot.kinetic_energy_),
			atom_positions_(snapshot.atom_positions_),
			atom_velocities_(snapshot.atom_velocities_),
			atom_forces_(snapshot.atom_forces_),
			valid_(snapshot.valid_)
	{
	}

	// The destructor just deletes all STL components (superfluous and just
	// for clarity)
	SnapShot::~SnapShot()
		throw()
	{
		clear();

		valid_ = false;
	}

	// The assignment operator
	const SnapShot& SnapShot::operator = (const SnapShot& snapshot)
		throw()
	{
		valid_ = snapshot.valid_;
		index_ = snapshot.index_;
		number_of_atoms_ = snapshot.number_of_atoms_;
		potential_energy_ = snapshot.potential_energy_;
		kinetic_energy_ = snapshot.kinetic_energy_;
		atom_positions_ = snapshot.atom_positions_;
		atom_velocities_ = snapshot.atom_velocities_;
		atom_forces_ = snapshot.atom_forces_;

		return *this;
	}

	void SnapShot::clear()
		throw()
	{
		valid_ = false;
		index_ = 0;
		number_of_atoms_ = 0;
		potential_energy_ = 0.0;
		kinetic_energy_ = 0.0;

		atom_positions_.clear();
		atom_velocities_.clear();
		atom_forces_.clear();
	}

	bool SnapShot::operator == (const SnapShot& snapshot) const
		throw()
	{
		return ((valid_ == snapshot.valid_)
			&& (index_ == snapshot.index_)
			&& (number_of_atoms_ == snapshot.number_of_atoms_)
			&& (potential_energy_ == snapshot.potential_energy_)
			&& (kinetic_energy_ == snapshot.kinetic_energy_)
			&& (atom_positions_ == snapshot.atom_positions_)
			&& (atom_velocities_ == snapshot.atom_velocities_)
			&& (atom_forces_ == snapshot.atom_forces_));
	}

	bool SnapShot::isValid() const
		throw()
	{
		return valid_;
	}

	void SnapShot::setIndex(Size index)
		throw()
	{
		index_ = index;
	}

	Size SnapShot::getIndex() const
		throw()
	{
		return index_;
	}

	void SnapShot::setNumberOfAtoms(Size number_of_atoms)
		throw()
	{
		number_of_atoms_ = number_of_atoms;
	}

	Size SnapShot::getNumberOfAtoms() const
		throw()
	{
		return number_of_atoms_;
	}

	DoubleReal SnapShot::getPotentialEnergy() const
		throw()
	{
		return potential_energy_;
	}

	void SnapShot::setPotentialEnergy(DoubleReal potential_energy)
		throw()
	{
		potential_energy_ = potential_energy;
	}

	DoubleReal SnapShot::getKineticEnergy() const
		throw()
	{
		return kinetic_energy_;
	}

	void SnapShot::setKineticEnergy(DoubleReal kinetic_energy)
		throw()
	{
		kinetic_energy_ = kinetic_energy;
	}

	void SnapShot::setAtomPositions(const ::std::vector<Vector3>& atom_postions)
		throw()
	{
		atom_positions_ = atom_postions;
	}

	const ::std::vector<Vector3>& SnapShot::getAtomPositions() const
		throw()
	{
		return atom_positions_;
	}

	void SnapShot::setAtomVelocitites(const ::std::vector<Vector3>& atom_velocities)
		throw()
	{
		atom_velocities_ = atom_velocities;
	}

	const ::std::vector<Vector3>& SnapShot::getAtomVelocities() const
		throw()
	{
		return atom_velocities_;
	}

	void SnapShot::setAtomForces(const ::std::vector<Vector3>& atom_forces)
		throw()
	{
		atom_forces_ = atom_forces;
	}

	const ::std::vector<Vector3>& SnapShot::getAtomForces() const
		throw()
	{
		return atom_forces_;
	}

	void SnapShot::takeSnapShot(const System& system)
		throw(Exception::OutOfMemory)
	{
		number_of_atoms_ = system.countAtoms();

		AtomIterator atom_it = system.beginAtom();

		// reserve memory
		atom_positions_.resize(number_of_atoms_);
		atom_velocities_.resize(number_of_atoms_);
		atom_forces_.resize(number_of_atoms_);

		// This is the data section of the snapshot object
		for (Size i = 0; +atom_it; ++atom_it, ++i)
		{
			atom_positions_[i] = atom_it->getPosition();
			atom_velocities_[i] = atom_it->getVelocity();
			atom_forces_[i] = atom_it->getForce();
		}
	}

	void SnapShot::applySnapShot(System& system) const
		throw()
	{
		if (system.countAtoms() != number_of_atoms_)
		{
			Log.error () << "SnapShot::applySnapShot(): "
				<< "Atom counts do not match: System: " << system.countAtoms()
				<< " SnapShot: " << number_of_atoms_ << endl;
			return;
		}

		AtomIterator atom_it = system.beginAtom();

		for (Size i = 0; +atom_it; ++atom_it, ++i)
		{
			if (atom_positions_.size() > 0)
			{
				atom_it->setPosition(atom_positions_[i]);
			}
			if (atom_velocities_.size() > 0)
			{
				atom_it->setVelocity(atom_velocities_[i]);
			}
			if (atom_forces_.size() > 0)
			{
				atom_it->setForce(atom_forces_[i]);
			}
		}
	}

	void SnapShot::getAtomPositions(const System& system)
		throw(Exception::OutOfMemory)
	{
		// obtain the number of atoms
		number_of_atoms_ = system.countAtoms();

		// reserve memory
		atom_positions_.resize(number_of_atoms_);

		// copy data
		AtomIterator atom_it = system.beginAtom();
		for (Size i = 0; +atom_it; ++atom_it, ++i)
		{
			atom_positions_[i] = atom_it->getPosition();
		}
	}

	void SnapShot::setAtomPositions(System& system) const
		throw()
	{
		if (system.countAtoms() != number_of_atoms_)
		{
			Log.error () << "SnapShot::setAtomPositions(): "
				<< "Atom counts do not match: System: " << system.countAtoms()
				<< " SnapShot: " << number_of_atoms_ << endl;
			return;
		}

		AtomIterator atom_it = system.beginAtom();

		for (Size i = 0; +atom_it; ++atom_it, ++i)
		{
			atom_it->setPosition(atom_positions_[i]);
		}
	}

	void SnapShot::getAtomVelocities(const System& system)
		throw(Exception::OutOfMemory)
	{
		number_of_atoms_ = system.countAtoms();
		atom_velocities_.resize(number_of_atoms_);
		AtomIterator atom_it = system.beginAtom();
		for (Size i = 0; +atom_it; ++atom_it, ++i)
		{
			atom_velocities_[i] = atom_it->getVelocity();
		}
	}

	void SnapShot::setAtomVelocitites(System& system) const
		throw()
	{
		if (system.countAtoms() != number_of_atoms_)
		{
			Log.error () << "SnapShot::setAtomVelocitites(): "
				<< "Atom counts do not match: System: " << system.countAtoms()
				<< " SnapShot: " << number_of_atoms_ << endl;
			return;
		}

		AtomIterator atom_it = system.beginAtom();

		// This is the data section of the snapshot object
		for (Size i = 0; +atom_it; ++atom_it, ++i)
		{
			atom_it->setVelocity(atom_velocities_[i]);
		}
	}

	void SnapShot::getAtomForces(const System& system)
		throw(Exception::OutOfMemory)
	{
		number_of_atoms_ = system.countAtoms();
		atom_forces_.resize(number_of_atoms_);
		AtomIterator atom_it = system.beginAtom();
		for (Size i = 0; +atom_it; ++atom_it, ++i)
		{
			atom_forces_[i] = atom_it->getForce();
		}
	}

	void SnapShot::setAtomForces(System& system) const
		throw()
	{
		if (system.countAtoms() != number_of_atoms_)
		{
			Log.error () << "SnapShot::setAtomForces(): "
				<< "Atom counts do not match: System: " << system.countAtoms()
				<< " SnapShot: " << number_of_atoms_ << endl;
			return;
		}

		AtomIterator atom_it = system.beginAtom();

		// This is the data section of the snapshot object
		for (Size i = 0; +atom_it; ++atom_it, ++i)
		{
			atom_it->setForce(atom_forces_[i]);
		}
	}

	::std::ostream& operator << (::std::ostream& os, const SnapShot& ss)
	{
		os << ss.getNumberOfAtoms() << endl;

		vector<Vector3> data = ss.getAtomPositions();

		vector<Vector3>::const_iterator it = data.begin();
		for (; it != data.end(); ++it)
		{
			os << *it << endl;
		}

		data = ss.getAtomVelocities();
		for (it = data.begin(); it != data.end(); ++it)
		{
			os << *it << endl;
		}

		data = ss.getAtomForces();
		for (it = data.begin(); it != data.end(); ++it)
		{
			os << *it << endl;
		}
		return os;
	}

	::std::istream& operator >> (::std::istream& is, SnapShot& ss)
	{
		// BAUSTELLE:
		// this istream does not check anything!

		Size number;
		is >> number;

		vector<Vector3> data;
		data.resize(number);

		for (Size i = 0; i < number; ++i)
		{
			is >> data[i];
		}
		ss.setAtomPositions(data);

		for (Size i = 0; i < number; ++i)
		{
			is >> data[i];
		}
		ss.setAtomVelocitites(data);

		for (Size i = 0; i < number; ++i)
		{
			is >> data[i];
		}
		ss.setAtomForces(data);
		return is;
	}

}	// end of namespace BALL
<|endoftext|>


<commit_before>#include "gtest/gtest.h"

#include "comp_math.h"
#include "composition.h"
#include "cyc_limits.h"
#include "error.h"

namespace cm = cyclus::compmath;
using cyclus::Composition;
using cyclus::CompMap;

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, SubSame) {
  CompMap v;
  v[922350000] = 1;
  v[922380000] = 2;

  v = cm::Sub(v, v);

  for (CompMap::iterator it = v.begin(); it != v.end(); ++it) {
    EXPECT_DOUBLE_EQ(0, it->second);
  }
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, SubCloseSize) {
  CompMap v;
  double qty = 0.1;
  v[922350000] = qty;

  CompMap v2(v);
  cm::Normalize(&v2, qty - cyclus::eps_rsrc());

  CompMap remainder;

  remainder = cm::Sub(v, v2);
  CompMap::iterator it;
  for (it = remainder.begin(); it != remainder.end(); ++it) {
    double expected = cyclus::eps_rsrc();
    EXPECT_FLOAT_EQ(expected, it->second);
  }
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, SubCloseComp) {
  double qty = 0.1;

  CompMap v;
  v[922350000] = 1;
  v[922410000] = 1;
  v[822080000] = 1;
  cm::Normalize(&v, qty);

  CompMap closev(v);
  closev[92241] -= cyclus::eps_rsrc();

  CompMap remainder;
  remainder = cm::Sub(v, closev);

  CompMap::iterator it;
  for (it = remainder.begin(); it != remainder.end(); ++it) {
    double expected = 0;
    if (it->first == 92241) {
      expected = v[92241] - closev[92241];
    }
    EXPECT_FLOAT_EQ(expected, it->second);
  }
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, Sum) {
  CompMap v;
  v[1] = 1.0;
  v[2] = 2.0;
  v[3] = 3.0;

  EXPECT_FLOAT_EQ(6.0, cm::Sum(&v));

}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, ApplyThresholdZero) {
  // if the threshold is 0, applying the threshold should do nothing
  CompMap v;
  v[1] = 1.0;
  v[2] = 2.0;
  v[3] = 3.0;

  EXPECT_NO_THROW(cm::ApplyThreshold(&v, 0));

  CompMap::iterator it;
  for (it = v.begin(); it != v.end(); ++it) {
    EXPECT_FLOAT_EQ(v[it->first], it->second);
  }
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, ApplyThresholdInf) {
  CompMap v;
  v[1] = 1.0;
  v[2] = 2.0;
  v[3] = 3.0;

  // if the threshold is infinit, applying it should zero any vector
  double infty = std::numeric_limits<double>::infinity();
  EXPECT_NO_THROW(cm::ApplyThreshold(&v, infty));

  CompMap::iterator it;
  for (it = v.begin(); it != v.end(); ++it) {
    EXPECT_FLOAT_EQ(0, it->second);
  }
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, ApplyThresholdNegative) {
  CompMap v;
  v[1] = 1.0;
  v[2] = 2.0;
  v[3] = 3.0;

  EXPECT_THROW(cm::ApplyThreshold(&v, -1), cyclus::ValueError);
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, ApplyThresholdMedium) {
  CompMap v;
  v[1] = 1.0;
  v[2] = 2.0;
  v[3] = 3.0;

  // if the threshold is in a reasonable range, it should zero small vals
  CompMap::iterator it;
  for (it = v.begin(); it != v.end(); ++it) {
    EXPECT_NO_THROW(cm::ApplyThreshold(&v, it->second));
    EXPECT_FLOAT_EQ(0, v[it->first]);
  }
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, ApplyThresholdSeqIter) {
  // tests for a bug caused by improper iteration over CompMap
  CompMap v;
  v[0] = 2.0;
  v[1] = 0.1;
  v[2] = 0.1;
  v[3] = 0.1;
  v[4] = 3.0;

  EXPECT_NO_THROW(cm::ApplyThreshold(&v, 1.0));
  EXPECT_FLOAT_EQ(2, v[0]);
  EXPECT_FLOAT_EQ(0, v[1]);
  EXPECT_FLOAT_EQ(0, v[2]);
  EXPECT_FLOAT_EQ(0, v[3]);
  EXPECT_FLOAT_EQ(3, v[4]);
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, AlmostEq) {
  CompMap v1;
  v1[1] = 0.1;
  v1[2] = 0.1;
  v1[3] = 0.1;

  CompMap v2(v1);
  CompMap::iterator it;
  for (it = v2.begin(); it != v2.end(); ++it) {
    v2[it->first] = 0.11;
  }

  EXPECT_TRUE(cm::AlmostEq(v1, v2, 0.1));
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, AlmostEqualZeroEntry) {
  CompMap v1;
  v1[1] = 0.1;
  v1[2] = 0.1;
  v1[3] = 0.1;

  CompMap v2(v1);
  CompMap::iterator it;
  for (it = v1.begin(); it != v1.end(); ++it) {
    v1[it->first] = 0;
  }

  EXPECT_TRUE(cm::AlmostEq(v1, v2, 1.1));
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, AlmostEqualNegThresh) {
  CompMap v1, v2;
  EXPECT_THROW(cm::AlmostEq(v1, v2, -1.0), cyclus::ValueError);
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, SimpleAdd) {
  CompMap v1;
  v1[1] = 1.1;
  v1[2] = 2.2;
  v1[3] = 3.3;
  cm::Normalize(&v1, 1.0);
  CompMap v2(v1);
  cm::Normalize(&v2, 2.0);

  CompMap result = cm::Add(v1, v2);
  cm::Normalize(&result);
  EXPECT_TRUE(cm::AlmostEq(v1, result, 1e-15));
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, Add) {
  CompMap v1;
  v1[1] = 1.0;
  v1[2] = 2.0;
  v1[3] = 3.0;

  CompMap v2;
  v2[1] = 1.1;
  v2[2] = 2.2;
  v2[3] = 3.3;

  CompMap expect;
  expect[1] = 2.1;
  expect[2] = 4.2;
  expect[3] = 6.3;

  CompMap result;

  result = cm::Add(v1, v2);
  EXPECT_TRUE(cm::AlmostEq(result, expect, 0));
  for (CompMap::iterator it = result.begin();
       it != result.end(); it++) {
    EXPECT_DOUBLE_EQ(it->second, expect[it->first]);
  }
}
<commit_msg>moved errant ampersand<commit_after>#include "gtest/gtest.h"

#include "comp_math.h"
#include "composition.h"
#include "cyc_limits.h"
#include "error.h"

namespace cm = cyclus::compmath;
using cyclus::Composition;
using cyclus::CompMap;

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, SubSame) {
  CompMap v;
  v[922350000] = 1;
  v[922380000] = 2;

  v = cm::Sub(v, v);

  for (CompMap::iterator it = v.begin(); it != v.end(); ++it) {
    EXPECT_DOUBLE_EQ(0, it->second);
  }
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, SubCloseSize) {
  CompMap v;
  double qty = 0.1;
  v[922350000] = qty;

  CompMap v2(v);
  cm::Normalize(&v2, qty - cyclus::eps_rsrc());

  CompMap remainder;

  remainder = cm::Sub(v, v2);
  CompMap::iterator it;
  for (it = remainder.begin(); it != remainder.end(); ++it) {
    double expected = cyclus::eps_rsrc();
    EXPECT_FLOAT_EQ(expected, it->second);
  }
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, SubCloseComp) {
  double qty = 0.1;

  CompMap v;
  v[922350000] = 1;
  v[922410000] = 1;
  v[822080000] = 1;
  cm::Normalize(&v, qty);

  CompMap closev(v);
  closev[92241] -= cyclus::eps_rsrc();

  CompMap remainder;
  remainder = cm::Sub(v, closev);

  CompMap::iterator it;
  for (it = remainder.begin(); it != remainder.end(); ++it) {
    double expected = 0;
    if (it->first == 92241) {
      expected = v[92241] - closev[92241];
    }
    EXPECT_FLOAT_EQ(expected, it->second);
  }
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, Sum) {
  CompMap v;
  v[1] = 1.0;
  v[2] = 2.0;
  v[3] = 3.0;

  EXPECT_FLOAT_EQ(6.0, cm::Sum(v));

}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, ApplyThresholdZero) {
  // if the threshold is 0, applying the threshold should do nothing
  CompMap v;
  v[1] = 1.0;
  v[2] = 2.0;
  v[3] = 3.0;

  EXPECT_NO_THROW(cm::ApplyThreshold(&v, 0));

  CompMap::iterator it;
  for (it = v.begin(); it != v.end(); ++it) {
    EXPECT_FLOAT_EQ(v[it->first], it->second);
  }
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, ApplyThresholdInf) {
  CompMap v;
  v[1] = 1.0;
  v[2] = 2.0;
  v[3] = 3.0;

  // if the threshold is infinit, applying it should zero any vector
  double infty = std::numeric_limits<double>::infinity();
  EXPECT_NO_THROW(cm::ApplyThreshold(&v, infty));

  CompMap::iterator it;
  for (it = v.begin(); it != v.end(); ++it) {
    EXPECT_FLOAT_EQ(0, it->second);
  }
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, ApplyThresholdNegative) {
  CompMap v;
  v[1] = 1.0;
  v[2] = 2.0;
  v[3] = 3.0;

  EXPECT_THROW(cm::ApplyThreshold(&v, -1), cyclus::ValueError);
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, ApplyThresholdMedium) {
  CompMap v;
  v[1] = 1.0;
  v[2] = 2.0;
  v[3] = 3.0;

  // if the threshold is in a reasonable range, it should zero small vals
  CompMap::iterator it;
  for (it = v.begin(); it != v.end(); ++it) {
    EXPECT_NO_THROW(cm::ApplyThreshold(&v, it->second));
    EXPECT_FLOAT_EQ(0, v[it->first]);
  }
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, ApplyThresholdSeqIter) {
  // tests for a bug caused by improper iteration over CompMap
  CompMap v;
  v[0] = 2.0;
  v[1] = 0.1;
  v[2] = 0.1;
  v[3] = 0.1;
  v[4] = 3.0;

  EXPECT_NO_THROW(cm::ApplyThreshold(&v, 1.0));
  EXPECT_FLOAT_EQ(2, v[0]);
  EXPECT_FLOAT_EQ(0, v[1]);
  EXPECT_FLOAT_EQ(0, v[2]);
  EXPECT_FLOAT_EQ(0, v[3]);
  EXPECT_FLOAT_EQ(3, v[4]);
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, AlmostEq) {
  CompMap v1;
  v1[1] = 0.1;
  v1[2] = 0.1;
  v1[3] = 0.1;

  CompMap v2(v1);
  CompMap::iterator it;
  for (it = v2.begin(); it != v2.end(); ++it) {
    v2[it->first] = 0.11;
  }

  EXPECT_TRUE(cm::AlmostEq(v1, v2, 0.1));
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, AlmostEqualZeroEntry) {
  CompMap v1;
  v1[1] = 0.1;
  v1[2] = 0.1;
  v1[3] = 0.1;

  CompMap v2(v1);
  CompMap::iterator it;
  for (it = v1.begin(); it != v1.end(); ++it) {
    v1[it->first] = 0;
  }

  EXPECT_TRUE(cm::AlmostEq(v1, v2, 1.1));
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, AlmostEqualNegThresh) {
  CompMap v1, v2;
  EXPECT_THROW(cm::AlmostEq(v1, v2, -1.0), cyclus::ValueError);
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, SimpleAdd) {
  CompMap v1;
  v1[1] = 1.1;
  v1[2] = 2.2;
  v1[3] = 3.3;
  cm::Normalize(&v1, 1.0);
  CompMap v2(v1);
  cm::Normalize(&v2, 2.0);

  CompMap result = cm::Add(v1, v2);
  cm::Normalize(&result);
  EXPECT_TRUE(cm::AlmostEq(v1, result, 1e-15));
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
TEST(CompMathTests, Add) {
  CompMap v1;
  v1[1] = 1.0;
  v1[2] = 2.0;
  v1[3] = 3.0;

  CompMap v2;
  v2[1] = 1.1;
  v2[2] = 2.2;
  v2[3] = 3.3;

  CompMap expect;
  expect[1] = 2.1;
  expect[2] = 4.2;
  expect[3] = 6.3;

  CompMap result;

  result = cm::Add(v1, v2);
  EXPECT_TRUE(cm::AlmostEq(result, expect, 0));
  for (CompMap::iterator it = result.begin();
       it != result.end(); it++) {
    EXPECT_DOUBLE_EQ(it->second, expect[it->first]);
  }
}
<|endoftext|>


<commit_before>/******************************************************************************
 * This file is part of the CutePaste project
 * Copyright (c) 2013 Laszlo Papp <lpapp@kde.org>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include <QSslError>
#include <QNetworkAccessManager>
#include <QNetworkRequest>
#include <QNetworkReply>
#include <QTcpSocket>

#include <QJsonDocument>
#include <QJsonObject>
#include <QJsonParseError>
#include <QFile>
#include <QScopedPointer>
#include <QTextStream>
#include <QStringList>
#include <QCoreApplication>

#include <QDebug>

int main(int argc, char **argv)
{
    QCoreApplication application(argc, argv);
    application.setOrganizationName("CutePaste");
    application.setApplicationName("CutePaste Desktop Console Frontend");

    QTextStream standardOutputStream(stdout);
    QFile dataFile;
    QString firstArgument = QCoreApplication::arguments().size() < 2 ? QString() : QCoreApplication::arguments().at(1);
    if (!firstArgument.isEmpty()) {
        dataFile.setFileName(firstArgument);
        dataFile.open(QIODevice::ReadOnly);
    } else {
        dataFile.open(stdin, QIODevice::ReadOnly);
    }

    QByteArray pasteTextByteArray = dataFile.readAll();

    QJsonObject requestJsonObject;
    requestJsonObject.insert(QStringLiteral("data"), QString::fromUtf8(pasteTextByteArray));
    requestJsonObject.insert(QStringLiteral("language"), QStringLiteral("text"));

    QJsonDocument requestJsonDocument(requestJsonObject);

    QNetworkRequest networkRequest;
    networkRequest.setAttribute(QNetworkRequest::DoNotBufferUploadDataAttribute, true);
    networkRequest.setHeader(QNetworkRequest::ContentTypeHeader, "application/json");
    networkRequest.setUrl(QUrl("http://sandbox.pastebin.kde.org/api/json/create"));

    QNetworkAccessManager networkAccessManager;
    QScopedPointer<QNetworkReply> networkReplyScopedPointer(networkAccessManager.post(networkRequest, requestJsonDocument.toJson()));
    QObject::connect(networkReplyScopedPointer.data(), &QNetworkReply::finished, [&]() {

        QJsonParseError jsonParseError;
        QByteArray replyJsonByteArray = networkReplyScopedPointer->readAll();
        QJsonDocument replyJsonDocument = QJsonDocument::fromJson(replyJsonByteArray, &jsonParseError);
        if (!replyJsonDocument.isObject()) {
            qDebug() << "Reply error:" << jsonParseError.errorString();
            return;
        }

        QJsonObject replyJsonObject = replyJsonDocument.object();
        QJsonValue identifierValue = replyJsonObject.value(QStringLiteral("result")).toObject().value(QStringLiteral("id"));

        if (identifierValue.isString())
            endl(standardOutputStream << "http://sandbox.pastebin.kde.org/" << identifierValue.toString());

        QCoreApplication::quit();
    });

    QObject::connect(networkReplyScopedPointer.data(), static_cast<void (QNetworkReply::*)(QNetworkReply::NetworkError)>(&QNetworkReply::error), [&](QNetworkReply::NetworkError networkReplyError) {
        if (networkReplyError != QNetworkReply::NoError)
            endl(standardOutputStream << networkReplyScopedPointer->errorString());
    });

    QObject::connect(networkReplyScopedPointer.data(), &QNetworkReply::sslErrors, [&](QList<QSslError> networkReplySslErrors) {
        if (!networkReplySslErrors.isEmpty()) {
            foreach (const QSslError &networkReplySslError, networkReplySslErrors)
                endl(standardOutputStream << networkReplySslError.errorString());
        }
    });

    bool returnValue = application.exec();

    dataFile.close();
    if (dataFile.error() != QFileDevice::NoError)
        endl(standardOutputStream << dataFile.errorString());

    return returnValue;
}
<commit_msg>Use the new API url and establish an internal variable for flexibility<commit_after>/******************************************************************************
 * This file is part of the CutePaste project
 * Copyright (c) 2013 Laszlo Papp <lpapp@kde.org>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include <QSslError>
#include <QNetworkAccessManager>
#include <QNetworkRequest>
#include <QNetworkReply>
#include <QTcpSocket>

#include <QJsonDocument>
#include <QJsonObject>
#include <QJsonParseError>
#include <QFile>
#include <QScopedPointer>
#include <QTextStream>
#include <QStringList>
#include <QCoreApplication>

#include <QDebug>

int main(int argc, char **argv)
{
    QCoreApplication application(argc, argv);
    application.setOrganizationName("CutePaste");
    application.setApplicationName("CutePaste Desktop Console Frontend");

    QTextStream standardOutputStream(stdout);
    QFile dataFile;
    QString firstArgument = QCoreApplication::arguments().size() < 2 ? QString() : QCoreApplication::arguments().at(1);
    if (!firstArgument.isEmpty()) {
        dataFile.setFileName(firstArgument);
        dataFile.open(QIODevice::ReadOnly);
    } else {
        dataFile.open(stdin, QIODevice::ReadOnly);
    }

    QByteArray pasteTextByteArray = dataFile.readAll();

    QJsonObject requestJsonObject;
    requestJsonObject.insert(QStringLiteral("data"), QString::fromUtf8(pasteTextByteArray));
    requestJsonObject.insert(QStringLiteral("language"), QStringLiteral("text"));

    QJsonDocument requestJsonDocument(requestJsonObject);

    QString baseUrlString = QStringLiteral("http://pastebin.kde.org");

    QNetworkRequest networkRequest;
    networkRequest.setAttribute(QNetworkRequest::DoNotBufferUploadDataAttribute, true);
    networkRequest.setHeader(QNetworkRequest::ContentTypeHeader, "application/json");
    networkRequest.setUrl(QUrl(baseUrlString + "/api/json/create"));

    QNetworkAccessManager networkAccessManager;
    QScopedPointer<QNetworkReply> networkReplyScopedPointer(networkAccessManager.post(networkRequest, requestJsonDocument.toJson()));
    QObject::connect(networkReplyScopedPointer.data(), &QNetworkReply::finished, [&]() {

        QJsonParseError jsonParseError;
        QByteArray replyJsonByteArray = networkReplyScopedPointer->readAll();
        QJsonDocument replyJsonDocument = QJsonDocument::fromJson(replyJsonByteArray, &jsonParseError);
        if (!replyJsonDocument.isObject()) {
            qDebug() << "Reply error:" << jsonParseError.errorString();
            return;
        }

        QJsonObject replyJsonObject = replyJsonDocument.object();
        QJsonValue identifierValue = replyJsonObject.value(QStringLiteral("result")).toObject().value(QStringLiteral("id"));

        if (identifierValue.isString())
            endl(standardOutputStream << baseUrlString << identifierValue.toString());

        QCoreApplication::quit();
    });

    QObject::connect(networkReplyScopedPointer.data(), static_cast<void (QNetworkReply::*)(QNetworkReply::NetworkError)>(&QNetworkReply::error), [&](QNetworkReply::NetworkError networkReplyError) {
        if (networkReplyError != QNetworkReply::NoError)
            endl(standardOutputStream << networkReplyScopedPointer->errorString());
    });

    QObject::connect(networkReplyScopedPointer.data(), &QNetworkReply::sslErrors, [&](QList<QSslError> networkReplySslErrors) {
        if (!networkReplySslErrors.isEmpty()) {
            foreach (const QSslError &networkReplySslError, networkReplySslErrors)
                endl(standardOutputStream << networkReplySslError.errorString());
        }
    });

    bool returnValue = application.exec();

    dataFile.close();
    if (dataFile.error() != QFileDevice::NoError)
        endl(standardOutputStream << dataFile.errorString());

    return returnValue;
}
<|endoftext|>


<commit_before>#include <catch2/catch.hpp>

#include <vector>
#include <optional>

struct Block
{
  size_t offset;
  size_t size;
};

struct TLSFSizeClass
{
  size_t sizeClass;
  uint64_t slBitmap;
  std::vector<std::vector<Block>> freeBlocks;
};

struct TLSFControl
{
  uint64_t flBitmap;
  std::vector<TLSFSizeClass> sizeclasses;
};

class TLSFAllocator
{
  unsigned fli; // first level index
  unsigned sli; // second level index, typically 5
  unsigned sli_count; // second level index, typically 5
  unsigned mbs; // minimum block size
  TLSFControl control;
  size_t m_size;

  int fls(uint64_t size)
  {
    unsigned long index;
    return _BitScanReverse64(&index, size) ? index : -1;
  }

  int ffs(uint64_t size)
  {
    unsigned long index;
    return _BitScanForward64(&index, size) ? index : -1;
  }

  void mapping(size_t size, unsigned& fl, unsigned& sl)
  {
    fl = fls(size);
    sl = ((size ^ (1<<fl)) >> (fl - sli));
  }

  void initialize()
  {
    fli = ffs(m_size);
    sli = 5;
    sli_count = 1 << sli;
    mbs = 16;
    control.flBitmap = 0;
    for (int i = 0; i < fli; ++i)
    {
      size_t sizeClass = 1 << i;
      std::vector<std::vector<Block>> vectors;
      for (int k = 0; k < sli; ++k)
      {
        vectors.push_back(std::vector<Block>());
      }
      control.sizeclasses.push_back(TLSFSizeClass{sizeClass, 0, vectors});
    }
  }
public:

  TLSFAllocator(size_t size)
    : m_size(size)
  {
    initialize();
  }

  std::optional<Block> allocate(size_t size)
  {
    unsigned fl, sl;
    mapping(size, fl, sl);
    printf("%zu -> fl %u sl %u\n", size, fl, sl);
    return {};
  }

  void free(Block block)
  {
    // immediately merge with existing free blocks.
  }
};

TEST_CASE("can allocate")
{
  TLSFAllocator tlsf(4);
  auto block = tlsf.allocate(5);
  REQUIRE(!block);
  block = tlsf.allocate(5);
  REQUIRE(block);
  if (block)
  {
    REQUIRE(block.value().size == 1);
  }
}<commit_msg>Wrote a really crappy tlsf implementation merging is bruteforced because I cannot use boundary tag technique splitting can also take time...<commit_after>#include <catch2/catch.hpp>

#include <vector>
#include <optional>

struct Block
{
  size_t offset;
  size_t size;
  operator bool()
  {
    return size != 0;
  }
};

class TLSFAllocator
{
  struct TLSFSizeClass
  {
    size_t sizeClass;
    uint64_t slBitmap;
    std::vector<std::vector<Block>> freeBlocks;
  };

  struct TLSFControl
  {
    uint64_t flBitmap;
    std::vector<TLSFSizeClass> sizeclasses;
  };

  unsigned fli; // first level index
  unsigned sli; // second level index, typically 5
  unsigned sli_count; // second level index, typically 5
  unsigned mbs; // minimum block size
  TLSFControl control;
  size_t m_size;

  int fls(uint64_t size)
  {
    unsigned long index;
    return _BitScanReverse64(&index, size) ? index : -1;
  }

  int ffs(uint64_t size)
  {
    unsigned long index;
    return _BitScanForward64(&index, size) ? index : -1;
  }

  void mapping(size_t size, unsigned& fl, unsigned& sl)
  {
    fl = fls(size);
    sl = ((size ^ (1<<fl)) >> (fl - sli));
    //printf("%zu -> fl %u sl %u\n", size, fl, sl);
  }

  void initialize()
  {
    fli = ffs(m_size);
    sli = 5;
    sli_count = 1 << sli;
    mbs = 16;
    control.flBitmap = 0;
    for (int i = 0; i <= fli; ++i)
    {
      size_t sizeClass = 1 << i;
      std::vector<std::vector<Block>> vectors;
      for (int k = 0; k < sli; ++k)
      {
        vectors.push_back(std::vector<Block>());
      }
      control.sizeclasses.push_back(TLSFSizeClass{sizeClass, 0, vectors});
    }
  }

  void remove_bit(uint64_t& value, int index)
  {
    value = value & (~(1 << index));
  }

  void set_bit(uint64_t& value, int index)
  {
    value |= (1 << index);
  }

  void insert(Block block, unsigned fl, unsigned sl)
  {
    auto& sizeClass = control.sizeclasses[fl];
    auto& secondLv = sizeClass.freeBlocks[sl];
    secondLv.push_back(block);
    set_bit(sizeClass.slBitmap, sl);
    set_bit(control.flBitmap, fl);
  }

  Block search_suitable_block(size_t size, unsigned fl, unsigned sl)
  {
    // first step, assume we got something at fl / sl location
    auto& sizeClass = control.sizeclasses[fl];
    auto& secondLv = sizeClass.freeBlocks[sl];
    if (!secondLv.empty() && sizeClass.sizeClass >= size) {
      auto block = secondLv.back();
      secondLv.pop_back();
      // remove bitmap bit
      if (secondLv.empty()) remove_bit(sizeClass.slBitmap, sl);
      if (sizeClass.slBitmap == 0) remove_bit(control.flBitmap, fl);
      return block;
    }
    else {
      // second step, scan bitmaps for empty slots
      auto mask = ~((1 << (fl+1))-1); // create mask to ignore first bits, could be wrong
      auto fl2 = ffs(control.flBitmap & mask);
      if (fl2 >= 0) {
        auto& sizeClass2 = control.sizeclasses[fl2];
        auto sl2 = ffs(sizeClass2.slBitmap);
        if (sl2 >= 0 && sizeClass2.sizeClass >= size) {
          assert(!sizeClass2.freeBlocks[sl2].empty());
          auto block = sizeClass2.freeBlocks[sl2].back();
          sizeClass2.freeBlocks[sl2].pop_back();
          // remove bitmap bit
          if (secondLv.empty()) remove_bit(sizeClass2.slBitmap, sl2);
          if (sizeClass2.slBitmap == 0) remove_bit(control.flBitmap, fl2);
          return block;
        }
      }
    }

    return {};
  }

  Block split(Block& block)
  {
    auto new_size = block.size / 2;
    Block new_block = {block.offset+new_size, new_size};
    block.size = new_size;
    return new_block;
  }

  Block merge(Block block)
  {
    // try to merge with some existing block???????????????
    auto otf = block.offset;
    auto otf2 = block.offset + block.size;
    // BRUTEFORCE
    auto fl = 0;
    for (auto&& firstLevel : control.sizeclasses)
    {
      auto sl = 0;
      for (auto&& secondLevel : firstLevel.freeBlocks)
      {
        auto iter = std::find_if(secondLevel.begin(), secondLevel.end(), [otf, otf2](Block b){
          return (b.offset+b.size == otf) || (b.offset == otf2);
        });
        if (iter != secondLevel.end())
        {
          auto rb = *iter;
          secondLevel.erase(iter);
          if (secondLevel.empty()) remove_bit(firstLevel.slBitmap, sl);
          if (firstLevel.slBitmap == 0) remove_bit(control.flBitmap, fl);

          if (rb.offset+rb.size == otf) {
            rb.size += block.size;
            return rb;
          }
          else if(rb.offset == otf2) {
            block.size += rb.size;
            return block;
          }
        }
        sl++;
      }
      fl++;
    }
    return block;
  }
public:
  TLSFAllocator(size_t size)
    : m_size(size)
  {
    initialize();
    unsigned fl, sl;
    mapping(size, fl, sl);
    insert({0, size}, fl, sl);
  }

  std::optional<Block> allocate(size_t size)
  {
    unsigned fl, sl, fl2, sl2;
    mapping(size, fl, sl);
    auto found_block = search_suitable_block(size,fl,sl);
    if (found_block) {
      while (found_block.size >= size*2)
      {
        auto remaining_block = split(found_block);
        mapping(remaining_block.size, fl2, sl2);
        insert(remaining_block, fl2, sl2); // O(1)
      }
      return found_block;
    }
    return {};
  }

  void free(Block block)
  {
    // immediately merge with existing free blocks.
    unsigned fl, sl;
    auto big_free_block = merge(block); // O(1)
    mapping(big_free_block.size, fl, sl);
    insert(big_free_block, fl, sl); // O(1)
  }
};

TEST_CASE("some basic allocation tests")
{
  TLSFAllocator tlsf(4);
  auto block = tlsf.allocate(5);
  REQUIRE_FALSE(block);
  block = tlsf.allocate(2);
  REQUIRE(block);
  if (block)
  {
    REQUIRE(block.value().size == 2);
  }
  tlsf.free(block.value());
  block = tlsf.allocate(4);
  REQUIRE(block);
  if (block)
  {
    REQUIRE(block.value().size == 4);
  }

  tlsf.free(block.value());
  block = tlsf.allocate(3);
  REQUIRE(block);
  if (block)
  {
    REQUIRE(block.value().size == 4);
  }

  tlsf.free(block.value());
  block = tlsf.allocate(2);
  auto block2 = tlsf.allocate(2);
  REQUIRE(block);
  REQUIRE(block2);
  if (block && block2)
  {
    REQUIRE(block.value().size == 2);
    REQUIRE(block2.value().size == 2);
  }
}<|endoftext|>


<commit_before><commit_msg>IndexedFaceSet triangle is introduced, but not tested<commit_after><|endoftext|>


<commit_before>#include "viewer/primitives/simple_geometry.hh"
#include "viewer/window_3d.hh"

#include "estimation/jet/jet_filter.hh"
#include "estimation/jet/jet_pose_opt.hh"

#include "estimation/jet/jet_rk4.hh"

#include "util/environment.hh"

#include "eigen.hh"
#include "sophus.hh"

namespace estimation {
namespace jet_filter {
namespace {
/*
void go() {
  setup();
  const auto view = viewer::get_window3d("Mr. Filter, filters");
  const auto geo = view->add_primitive<viewer::SimpleGeometry>();

  const jcc::Vec3 g(0.0, 0.0, 0.0);
  const SO3 r_sensor_from_body;
  const jcc::Vec3 t_sensor_from_body = jcc::Vec3(0.0, 0.0, 1.0);
  const SE3 sensor_from_body = SE3(r_sensor_from_body, t_sensor_from_body);

  State x;
  x.T_body_from_world = SE3(SO3(), jcc::Vec3(5.0, 0.0, 0.0));

  x.eps_dot[0] = 0.0;
  x.eps_dot[1] = 0.0;
  x.eps_dot[2] = 0.0;
  x.eps_dot[3] = 0.0;
  x.eps_dot[4] = 0.0;
  x.eps_dot[5] = 0.0;

  x.eps_ddot[0] = 0.0;
  x.eps_ddot[1] = 1.0;
  x.eps_ddot[2] = 0.0;
  x.eps_ddot[3] = 0.0;
  x.eps_ddot[4] = 0.0;
  x.eps_ddot[5] = 1.0;

  p = mock_parameters();
  const auto res = observe_accel(x, p);

  constexpr double dt = 0.01;
  while (!view->should_close()) {
    x = dynamics(x, dt);

    geo->add_axes({x.T_body_from_world.inverse()});
    const SE3 world_from_sensor = (sensor_from_body * x.T_body_from_world).inverse();
    geo->add_axes({world_from_sensor, 0.25});
    const auto res =
        observe_accel(x.T_body_from_world, x.eps_dot, x.eps_ddot, sensor_from_body, g);

    geo->add_ray(
        {world_from_sensor.translation(), world_from_sensor.so3() * res, res.norm()});

    geo->flip();

    view->spin_until_step();
  }
}
*/

Parameters mock_parameters() {
  const jcc::Vec3 g(0.0, 0.0, 4.1);
  const SE3 vehicle_from_sensor;
  Parameters p;
  p.T_sensor_from_body = vehicle_from_sensor;
  p.g_world = g;
  return p;
}

void setup() {
  const auto view = viewer::get_window3d("Mr. Filter, filters");
  view->set_target_from_world(
      SE3(SO3::exp(Eigen::Vector3d(-3.1415 * 0.5, 0.0, 0.0)), Eigen::Vector3d::Zero()));
  view->set_continue_time_ms(10);

  const auto background = view->add_primitive<viewer::SimpleGeometry>();
  const geometry::shapes::Plane ground{jcc::Vec3::UnitZ(), 0.0};
  background->add_plane({ground});
  background->flip();
}

void draw_states(viewer::SimpleGeometry& geo,
                 const std::vector<State>& states,
                 bool truth) {
  for (const auto& state : states) {
    const SE3 T_world_from_body = state.T_body_from_world.inverse();
    if (truth) {
      geo.add_axes({T_world_from_body, 1.0});
    } else {
      geo.add_axes({T_world_from_body, 0.5, 2.0, true});
    }
  }
}
}  // namespace

class JetOptimizer {
 public:
  JetOptimizer() {
    imu_id_ = pose_opt_.add_error_model<AccelMeasurement>(accel_error_model);
    fiducial_id_ = pose_opt_.add_error_model<FiducialMeasurement>(fiducial_error_model);
  }

  void measure_imu(const AccelMeasurement& meas, const TimePoint& t) {
    pose_opt_.add_measurement(meas, t, imu_id_);
  }

  void measure_fiducial(const FiducialMeasurement& meas, const TimePoint& t) {
    pose_opt_.add_measurement(meas, t, fiducial_id_);
  }

  JetPoseOptimizer::Solution solve(const std::vector<State> x, const Parameters& p) {
    const auto visitor = [](const JetPoseOptimizer::Solution& soln) {
      const auto view = viewer::get_window3d("Mr. Filter, filters");
      const auto geo = view->add_primitive<viewer::SimpleGeometry>();
      draw_states(*geo, soln.x, false);
      geo->flip();
      view->spin_until_step();
      geo->clear();
    };

    return pose_opt_.solve({x, p}, visitor);
  }

 private:
  JetPoseOptimizer pose_opt_{rk4_integrate};
  int imu_id_ = -1;
  int fiducial_id_ = -1;
};

void run_filter() {
  const Parameters true_params = mock_parameters();

  JetOptimizer jet_opt;

  FilterState<State> xp0;
  xp0.P.setIdentity();
  xp0.x.eps_ddot[0] = 0.0;

  State true_x = xp0.x;
  true_x.eps_dot[0] = 0.1;
  true_x.eps_dot[3] = -0.1;
  true_x.eps_dot[4] = 0.1;

  JetFilter jf(xp0);

  // AccelMeasurement imu_meas;
  // imu_meas.observed_acceleration[0] = 2.0;

  std::vector<State> ground_truth;
  std::vector<State> est_states;

  TimePoint start_time = {};
  constexpr auto dt = to_duration(0.5);

  constexpr int NUM_SIM_STEPS = 76;

  for (int k = 0; k < NUM_SIM_STEPS; ++k) {
    const TimePoint obs_time = (dt * k) + start_time;
    const AccelMeasurement imu_meas = {observe_accel(true_x, true_params)};
    std::cout << "Accel: " << imu_meas.observed_acceleration.transpose() << std::endl;

    jf.measure_imu(imu_meas, obs_time);
    jet_opt.measure_imu(imu_meas, obs_time);
    jf.free_run();
    est_states.push_back(jf.state().x);
    ground_truth.push_back(true_x);

    constexpr auto dt2 = to_duration(0.1);
    true_x = rk4_integrate(true_x, true_params, to_seconds(dt2));
    ground_truth.push_back(true_x);
    const FiducialMeasurement fiducial_meas = observe_fiducial(true_x, true_params);

    jf.measure_fiducial(fiducial_meas, obs_time + dt2);
    jet_opt.measure_fiducial(fiducial_meas, obs_time + dt2);
    jf.free_run();
    est_states.push_back(jf.state().x);

    const auto xp = jf.state();
    std::cout << "k: " << k << std::endl;
    std::cout << "\teps_dot   : " << xp.x.eps_dot.transpose() << std::endl;
    std::cout << "\teps_ddot  : " << xp.x.eps_ddot.transpose() << std::endl;
    std::cout << "\taccel_bias: " << xp.x.accel_bias.transpose() << std::endl;
    std::cout << "\tgyro_bias : " << xp.x.gyro_bias.transpose() << std::endl;

    std::cout << "\tModelled Error: "
              << accel_error_model(xp.x, imu_meas, true_params).transpose() << std::endl;
    const auto res = observe_accel(xp.x, mock_parameters());
    std::cout << "\tExpected Measurement: " << res.transpose() << std::endl;

    true_x = rk4_integrate(true_x, true_params, to_seconds(dt));
  }

  // Do the optimization
  setup();
  const auto view = viewer::get_window3d("Mr. Filter, filters");
  const auto geo = view->add_primitive<viewer::SimpleGeometry>();
  draw_states(*geo, ground_truth, true);

  const auto solution = jet_opt.solve(est_states, {});
  std::cout << "Optimized g: " << solution.p.g_world.transpose() << std::endl;
  std::cout << "Optimized T_sensor_from_body: "
            << solution.p.T_sensor_from_body.log().transpose() << std::endl;

  draw_states(*geo, solution.x, false);

  geo->flip();

  view->spin_until_step();
}

}  // namespace jet_filter
}  // namespace estimation

int main() {
  // estimation::jet_filter::go();
  estimation::jet_filter::run_filter();
}<commit_msg>Filter demo is working<commit_after>#include "viewer/primitives/simple_geometry.hh"
#include "viewer/window_3d.hh"

#include "estimation/jet/jet_filter.hh"
#include "estimation/jet/jet_pose_opt.hh"

#include "estimation/jet/jet_rk4.hh"

#include "util/environment.hh"

#include "eigen.hh"
#include "sophus.hh"

namespace estimation {
namespace jet_filter {
namespace {
/*
void go() {
  setup();
  const auto view = viewer::get_window3d("Mr. Filter, filters");
  const auto geo = view->add_primitive<viewer::SimpleGeometry>();

  const jcc::Vec3 g(0.0, 0.0, 0.0);
  const SO3 r_sensor_from_body;
  const jcc::Vec3 t_sensor_from_body = jcc::Vec3(0.0, 0.0, 1.0);
  const SE3 sensor_from_body = SE3(r_sensor_from_body, t_sensor_from_body);

  State x;
  x.T_body_from_world = SE3(SO3(), jcc::Vec3(5.0, 0.0, 0.0));

  x.eps_dot[0] = 0.0;
  x.eps_dot[1] = 0.0;
  x.eps_dot[2] = 0.0;
  x.eps_dot[3] = 0.0;
  x.eps_dot[4] = 0.0;
  x.eps_dot[5] = 0.0;

  x.eps_ddot[0] = 0.0;
  x.eps_ddot[1] = 1.0;
  x.eps_ddot[2] = 0.0;
  x.eps_ddot[3] = 0.0;
  x.eps_ddot[4] = 0.0;
  x.eps_ddot[5] = 1.0;

  p = mock_parameters();
  const auto res = observe_accel(x, p);

  constexpr double dt = 0.01;
  while (!view->should_close()) {
    x = dynamics(x, dt);

    geo->add_axes({x.T_body_from_world.inverse()});
    const SE3 world_from_sensor = (sensor_from_body * x.T_body_from_world).inverse();
    geo->add_axes({world_from_sensor, 0.25});
    const auto res =
        observe_accel(x.T_body_from_world, x.eps_dot, x.eps_ddot, sensor_from_body, g);

    geo->add_ray(
        {world_from_sensor.translation(), world_from_sensor.so3() * res, res.norm()});

    geo->flip();

    view->spin_until_step();
  }
}
*/

Parameters mock_parameters() {
  const jcc::Vec3 g(0.0, 0.0, -1.3);
  // const jcc::Vec3 t(0.1, 0.5, 0.1);
  const jcc::Vec3 t(0.0, 0.1, 0.25);
  // const jcc::Vec3 t = jcc::Vec3::Zero();
  const SO3 r_vehicle_from_sensor = SO3::exp(jcc::Vec3(0.0, 0.0, 0.0));
  const SE3 vehicle_from_sensor(r_vehicle_from_sensor, t);
  Parameters p;
  p.T_sensor_from_body = vehicle_from_sensor;
  p.g_world = g;
  return p;
}

void setup() {
  const auto view = viewer::get_window3d("Mr. Filter, filters");
  view->set_target_from_world(
      SE3(SO3::exp(Eigen::Vector3d(-3.1415 * 0.5, 0.0, 0.0)), Eigen::Vector3d::Zero()));
  view->set_continue_time_ms(10);

  const auto background = view->add_primitive<viewer::SimpleGeometry>();
  const geometry::shapes::Plane ground{jcc::Vec3::UnitZ(), 0.0};
  background->add_plane({ground});
  background->flip();
}

void draw_states(viewer::SimpleGeometry& geo,
                 const std::vector<State>& states,
                 bool truth) {
  const int n_states = static_cast<int>(states.size());
  for (int k = 0; k < n_states; ++k) {
    auto& state = states.at(k);
    const SE3 T_world_from_body = state.T_body_from_world.inverse();
    if (truth) {
      geo.add_axes({T_world_from_body, 0.1});
    } else {
      geo.add_axes({T_world_from_body, 0.05, 2.0, true});

      if (k < n_states - 1) {
        const auto& next_state = states.at(k + 1);
        const SE3 T_world_from_body_next = next_state.T_body_from_world.inverse();
        geo.add_line(
            {T_world_from_body.translation(), T_world_from_body_next.translation()});
      }
    }
  }
}

void print_state(const State& x) {
  std::cout << "\teps_dot   : " << x.eps_dot.transpose() << std::endl;
  std::cout << "\teps_ddot  : " << x.eps_ddot.transpose() << std::endl;
  std::cout << "\taccel_bias: " << x.accel_bias.transpose() << std::endl;
  std::cout << "\tgyro_bias : " << x.gyro_bias.transpose() << std::endl;
  // const auto res = observe_accel(xp.x, true_params);
  // std::cout << "\tExpected Measurement: " << res.transpose() << std::endl;
}

template <int dim, int row, int mat_size>
void set_diag_to_value(MatNd<mat_size, mat_size>& mat, double value) {
  mat.template block<dim, dim>(row, row) = (MatNd<dim, dim>::Identity() * value);
}
}  // namespace

class JetOptimizer {
 public:
  JetOptimizer() {
    MatNd<AccelMeasurement::DIM, AccelMeasurement::DIM> accel_cov;
    accel_cov.setZero();
    set_diag_to_value<AccelMeasurementDelta::observed_acceleration_error_dim,
                      AccelMeasurementDelta::observed_acceleration_error_ind>(accel_cov,
                                                                              0.01);

    MatNd<FiducialMeasurement::DIM, FiducialMeasurement::DIM> fiducial_cov;
    fiducial_cov.block<3, 3>(0, 0) = MatNd<3, 3>::Identity() * 0.001;
    fiducial_cov.block<3, 3>(3, 3) = MatNd<3, 3>::Identity() * 0.0001;

    MatNd<State::DIM, State::DIM> state_cov;
    state_cov.setZero();
    set_diag_to_value<StateDelta::accel_bias_error_dim, StateDelta::accel_bias_error_ind>(
        state_cov, 0.0001);
    set_diag_to_value<StateDelta::gyro_bias_error_dim, StateDelta::gyro_bias_error_ind>(
        state_cov, 0.0001);
    set_diag_to_value<StateDelta::eps_dot_error_dim, StateDelta::eps_dot_error_ind>(
        state_cov, 0.0001);
    set_diag_to_value<StateDelta::eps_ddot_error_dim, StateDelta::eps_ddot_error_ind>(
        state_cov, 0.0001);

    constexpr int T_error_dim = StateDelta::T_body_from_world_error_log_dim;
    constexpr int T_error_ind = StateDelta::T_body_from_world_error_log_ind;
    set_diag_to_value<T_error_dim, T_error_ind>(state_cov, 0.001);

    std::cout << "AAAAA" << std::endl;
    std::cout << accel_cov << std::endl;

    imu_id_ = pose_opt_.add_error_model<AccelMeasurement>(accel_error_model, accel_cov);
    fiducial_id_ = pose_opt_.add_error_model<FiducialMeasurement>(fiducial_error_model,
                                                                  fiducial_cov);
    pose_opt_.set_dynamics_cov(state_cov);
  }

  void measure_imu(const AccelMeasurement& meas, const TimePoint& t) {
    pose_opt_.add_measurement(meas, t, imu_id_);
  }

  void measure_fiducial(const FiducialMeasurement& meas, const TimePoint& t) {
    pose_opt_.add_measurement(meas, t, fiducial_id_);
  }

  JetPoseOptimizer::Solution solve(const std::vector<State> x, const Parameters& p) {
    const auto visitor = [](const JetPoseOptimizer::Solution& soln) {
      const auto view = viewer::get_window3d("Mr. Filter, filters");
      const auto geo = view->add_primitive<viewer::SimpleGeometry>();

      draw_states(*geo, soln.x, false);
      geo->flip();
      std::cout << "\tOptimized g: " << soln.p.g_world.transpose() << std::endl;
      std::cout << "\tOptimized T_sensor_from_body: "
                << soln.p.T_sensor_from_body.translation().transpose() << "; "
                << soln.p.T_sensor_from_body.so3().log().transpose() << std::endl;
      view->spin_until_step();
      geo->clear();
    };

    return pose_opt_.solve({x, p}, visitor);
  }

 private:
  JetPoseOptimizer pose_opt_{rk4_integrate};
  int imu_id_ = -1;
  int fiducial_id_ = -1;
};

void run_filter() {
  const Parameters true_params = mock_parameters();

  JetOptimizer jet_opt;

  FilterState<State> xp0;
  xp0.P.setIdentity();
  xp0.x.eps_dot[0] = 1.0;
  xp0.x.eps_dot[4] = -0.1;
  xp0.x.eps_dot[5] = -1.0;

  xp0.x.eps_ddot[1] = 0.01;
  xp0.x.eps_ddot[5] = 0.01;

  xp0.time_of_validity = {};

  State true_x = xp0.x;
  true_x.eps_dot[4] = 0.1;

  JetFilter jf(xp0);

  // AccelMeasurement imu_meas;
  // imu_meas.observed_acceleration[0] = 2.0;

  setup();
  const auto view = viewer::get_window3d("Mr. Filter, filters");
  const auto obs_geo = view->add_primitive<viewer::SimpleGeometry>();
  constexpr double sphere_size_m = 0.05;

  std::vector<State> ground_truth;
  std::vector<State> est_states;

  TimePoint start_time = {};
  constexpr auto dt = to_duration(0.1);

  constexpr int NUM_SIM_STEPS = 25;

  TimePoint current_time = start_time;

  for (int k = 0; k < NUM_SIM_STEPS; ++k) {
    if (k > 75 && k < 95) {
      true_x.eps_ddot[4] = 0.1;
      true_x.eps_ddot[3] = 0.1;
      true_x.eps_ddot[5] = -0.01;
      true_x.eps_ddot[1] = -0.01;
    } else if (k > 95) {
      true_x.eps_ddot.setZero();
    }

    //
    // Accelerometer Observation
    //
    // if ((k % 10 == 0) && k > 75) {
    if (true) {
      ground_truth.push_back(true_x);
      const AccelMeasurement imu_meas = {observe_accel(true_x, true_params)};
      std::cout << "Accel: " << imu_meas.observed_acceleration.transpose() << std::endl;

      jf.measure_imu(imu_meas, current_time);
      jet_opt.measure_imu(imu_meas, current_time);

      jf.free_run();
      est_states.push_back(jf.state().x);
      assert(jf.state().time_of_validity == current_time);

      const jcc::Vec4 accel_color(0.0, 0.7, 0.7, 0.8);
      obs_geo->add_sphere({jf.state().x.T_body_from_world.inverse().translation(),
                           sphere_size_m, accel_color});

      const SE3 world_from_body = jf.state().x.T_body_from_world.inverse();
      const jcc::Vec3 observed_accel_body_frame =
          (world_from_body.so3() * true_params.T_sensor_from_body.so3().inverse() *
           imu_meas.observed_acceleration);

      obs_geo->add_line(
          {world_from_body.translation(),
           world_from_body.translation() + (0.25 * observed_accel_body_frame),
           accel_color});
    }

    //
    // Fiducial Measurement
    //

    {
      constexpr auto dt2 = to_duration(0.05);
      true_x = rk4_integrate(true_x, true_params, to_seconds(dt2));
      current_time += dt2;

      ground_truth.push_back(true_x);
      const FiducialMeasurement fiducial_meas = observe_fiducial(true_x, true_params);

      jf.measure_fiducial(fiducial_meas, current_time);
      jet_opt.measure_fiducial(fiducial_meas, current_time);

      jf.free_run();
      est_states.push_back(jf.state().x);
      assert(jf.state().time_of_validity == current_time);
      obs_geo->add_sphere({jf.state().x.T_body_from_world.inverse().translation(),
                           sphere_size_m, jcc::Vec4(1.0, 0.0, 0.0, 1.0)});
    }

    //
    // Printout
    //
    {
      const auto xp = jf.state();
      std::cout << "k: " << k << std::endl;
      print_state(xp.x);
    }
    true_x = rk4_integrate(true_x, true_params, to_seconds(dt));
    current_time += dt;
  }

  // Do the optimization

  obs_geo->flip();
  const auto geo = view->add_primitive<viewer::SimpleGeometry>();
  draw_states(*geo, ground_truth, true);
  geo->flip();

  // const std::vector<State> crap_init(est_states.size(), xp0.x);
  // const auto solution = jet_opt.solve(crap_init, jf.parameters());
  const auto solution = jet_opt.solve(est_states, jf.parameters());
  // const auto solution = jet_opt.solve(ground_truth, jf.parameters());

  for (std::size_t k = 0; k < solution.x.size(); ++k) {
    const auto& state = solution.x.at(k);
    std::cout << "----- " << k << std::endl;
    print_state(state);
    std::cout << "\\\\\\\\\\\\" << std::endl;
    print_state(ground_truth.at(k));
  }
  std::cout << "Optimized g: " << solution.p.g_world.transpose() << std::endl;
  std::cout << "Optimized T_sensor_from_body: "
            << solution.p.T_sensor_from_body.translation().transpose() << "; "
            << solution.p.T_sensor_from_body.so3().log().transpose() << std::endl;

  draw_states(*geo, solution.x, false);

  view->spin_until_step();
}

}  // namespace jet_filter
}  // namespace estimation

int main() {
  // estimation::jet_filter::go();
  estimation::jet_filter::run_filter();
}<|endoftext|>