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

#include "NearestNeighbors.hpp"

#include <vector>

#include "BasicGeometry.hpp"
#include "Common.hpp"

#include "NoIndex.hpp"
#include "AabbIndex.hpp"
#include "CubeIndex.hpp"

#include "DomainAssertions.hpp"

using namespace std;

namespace geoIndex {

TEST(BuildIndex, NoIndex) {
    std::vector<Point> points;
    const Point testPoint({1, 2, 3});
    points.push_back(testPoint);

    NoIndex<Point> indexToBuild(points.size());  // The optimization parameter is irrelevant for the test,
                                                 // but it shows the correct usage.
    BuildIndex(points, indexToBuild);

    std::vector<IndexAndSquaredDistance<Point> > readBackThePoint;
    indexToBuild.pointsWithinDistance(testPoint, 150, readBackThePoint);

    ASSERT_FALSE(readBackThePoint.empty());
}

TEST(BuildIndex, AabbIndex) {
    std::vector<Point> points;
    const Point testPoint({1, 2, 3});
    points.push_back(testPoint);

    AabbIndex<Point> indexToBuild(points.size());  // The optimization parameter is irrelevant for the test,
                                                   // but it shows the correct usage.
    BuildIndex(points, indexToBuild);

    std::vector<IndexAndSquaredDistance<Point> > readBackThePoint;
    indexToBuild.pointsWithinDistance(testPoint, 150, readBackThePoint);

    ASSERT_FALSE(readBackThePoint.empty());
}

TEST(BuildIndex, CubeIndex) {
    std::vector<Point> points;
    const Point testPoint({1, 2, 3});
    points.push_back(testPoint);

    CubeIndex<Point> indexToBuild(10.0);  // The optimization parameter is irrelevant for the test,
                                          // but it shows the correct usage.
    BuildIndex(points, indexToBuild);

    std::vector<IndexAndSquaredDistance<Point> > readBackThePoint;
    indexToBuild.pointsWithinDistance(testPoint, 150, readBackThePoint);

    ASSERT_FALSE(readBackThePoint.empty());
}

/* Shortcuts some constant patameters*/
static void TestKNearest_NoIndex(const std::vector<Point>& points,
                                 std::vector<IndexAndSquaredDistance<Point> >& result
) {
    NoIndex<Point> geometryIndex;
    BuildIndex(points, geometryIndex);
    const Point referencePoint{0, 0, 0};
    const PointTraits<Point>::coordinate cullingDistance = 65;

    const size_t k = 2;

    KNearestNeighbor(
        geometryIndex,
        cullingDistance,
        referencePoint,
        k,
        result);
}

TEST(KNearestNeighbor, lessThanKPoints) {
    std::vector<Point> points;
    points.push_back(Point{0, 0, 0});

    std::vector<IndexAndSquaredDistance<Point> > result;
    TestKNearest_NoIndex(points, result);

    ASSERT_EQ(1, result.size());
}

TEST(KNearestNeighbor, KPoints) {
    std::vector<Point> points;
    const Point origin{0, 0, 0};
    points.push_back(origin);
    points.push_back(origin);

    std::vector<IndexAndSquaredDistance<Point> > result;
    TestKNearest_NoIndex(points, result);
    ASSERT_EQ(2, result.size());
}

TEST(KNearestNeighbor, MoreThanKPoints) {
    std::vector<Point> points;
    const Point origin{0, 0, 0};
    points.push_back(origin);
    points.push_back(origin);
    points.push_back(origin);
    points.push_back(origin);

    std::vector<IndexAndSquaredDistance<Point> > result;
    TestKNearest_NoIndex(points, result);
    ASSERT_EQ(2, result.size());

    ASSERT_EQ(2, result.size());
}

TEST(KNearestNeighbor, GenericCase) {
    std::vector<Point> points;
    const Point referencePoint{0, 0, 0};
    points.push_back(Point{70, 1, 2});
    points.push_back(Point{11, 1, 2});
    points.push_back(Point{7, 1, 2});
    points.push_back(Point{7, 2, 2});
    points.push_back(Point{7, 2, -225});

    NoIndex<Point> geometryIndex;
    BuildIndex(points, geometryIndex);

    const PointTraits<Point>::coordinate cullingDistance = 65;

    const size_t k = 3;
    std::vector<IndexAndSquaredDistance<Point> > result;

    KNearestNeighbor(
        geometryIndex,
        cullingDistance,
        referencePoint,
        k,
        result);

    ASSERT_EQ(3, result.size());
    ASSERT_INDEX_PRESENT(result, 2);
    ASSERT_INDEX_PRESENT(result, 3);
    ASSERT_INDEX_PRESENT(result, 1);
}

#ifdef GEO_INDEX_SAFETY_CHECKS

TEST(KNearestNeighbor, zeroDistance) {
    const std::vector<Point> points;
    const NoIndex<Point> geometryIndex;
    const PointTraits<Point>::coordinate cullingDistance = 0;

    const Point referencePoint{0, 0, 0};
    const size_t k = 4;
    std::vector<IndexAndSquaredDistance<Point> > result;

    ASSERT_ANY_THROW(
        KNearestNeighbor(
            geometryIndex,
            cullingDistance,
            referencePoint,
            k,
            result)
    );
}

TEST(KNearestNeighbor, zeroK) {
    const std::vector<Point> points;
    const NoIndex<Point> geometryIndex;
    const PointTraits<Point>::coordinate cullingDistance = 10;

    const Point referencePoint{0, 0, 0};
    const size_t k = 0;
    std::vector<IndexAndSquaredDistance<Point> > result;

    ASSERT_ANY_THROW(
        KNearestNeighbor(
            geometryIndex,
            cullingDistance,
            referencePoint,
            k,
            result)
    );
}

#endif
/** This class makes little sense. It is just to show how to use custom classes. */
class PointAndClick {
public:
    int x;
    int y;
    int z;
    void doNothing(){};
private:
    char whyNotACharHere;
    float someValue;
};

template<>
struct PointTraits<PointAndClick> {
    typedef int coordinate;
    typedef unsigned int index;
};

TEST(KNearestNeighbor, UserDefinedClasses) {
    NoIndex<PointAndClick> geometryIndex;
    std::vector<PointAndClick> points;
    BuildIndex(points, geometryIndex);
    PointAndClick referencePoint;

    const size_t k = 2;

    std::vector<IndexAndSquaredDistance<PointAndClick> > result;

    KNearestNeighbor(
        geometryIndex,
        2,
        referencePoint,
        k,
        result);
}

}<commit_msg>New compiler: discovered and fixed uninitialized values.<commit_after>#include "gtest/gtest.h"

#include "NearestNeighbors.hpp"

#include <vector>

#include "BasicGeometry.hpp"
#include "Common.hpp"

#include "NoIndex.hpp"
#include "AabbIndex.hpp"
#include "CubeIndex.hpp"

#include "DomainAssertions.hpp"

using namespace std;

namespace geoIndex {

TEST(BuildIndex, NoIndex) {
    std::vector<Point> points;
    const Point testPoint({1, 2, 3});
    points.push_back(testPoint);

    NoIndex<Point> indexToBuild(points.size());  // The optimization parameter is irrelevant for the test,
                                                 // but it shows the correct usage.
    BuildIndex(points, indexToBuild);

    std::vector<IndexAndSquaredDistance<Point> > readBackThePoint;
    indexToBuild.pointsWithinDistance(testPoint, 150, readBackThePoint);

    ASSERT_FALSE(readBackThePoint.empty());
}

TEST(BuildIndex, AabbIndex) {
    std::vector<Point> points;
    const Point testPoint({1, 2, 3});
    points.push_back(testPoint);

    AabbIndex<Point> indexToBuild(points.size());  // The optimization parameter is irrelevant for the test,
                                                   // but it shows the correct usage.
    BuildIndex(points, indexToBuild);

    std::vector<IndexAndSquaredDistance<Point> > readBackThePoint;
    indexToBuild.pointsWithinDistance(testPoint, 150, readBackThePoint);

    ASSERT_FALSE(readBackThePoint.empty());
}

TEST(BuildIndex, CubeIndex) {
    std::vector<Point> points;
    const Point testPoint({1, 2, 3});
    points.push_back(testPoint);

    CubeIndex<Point> indexToBuild(10.0);  // The optimization parameter is irrelevant for the test,
                                          // but it shows the correct usage.
    BuildIndex(points, indexToBuild);

    std::vector<IndexAndSquaredDistance<Point> > readBackThePoint;
    indexToBuild.pointsWithinDistance(testPoint, 150, readBackThePoint);

    ASSERT_FALSE(readBackThePoint.empty());
}

/* Shortcuts some constant patameters*/
static void TestKNearest_NoIndex(const std::vector<Point>& points,
                                 std::vector<IndexAndSquaredDistance<Point> >& result
) {
    NoIndex<Point> geometryIndex;
    BuildIndex(points, geometryIndex);
    const Point referencePoint{0, 0, 0};
    const PointTraits<Point>::coordinate cullingDistance = 65;

    const size_t k = 2;

    KNearestNeighbor(
        geometryIndex,
        cullingDistance,
        referencePoint,
        k,
        result);
}

TEST(KNearestNeighbor, lessThanKPoints) {
    std::vector<Point> points;
    points.push_back(Point{0, 0, 0});

    std::vector<IndexAndSquaredDistance<Point> > result;
    TestKNearest_NoIndex(points, result);

    ASSERT_EQ(1, result.size());
}

TEST(KNearestNeighbor, KPoints) {
    std::vector<Point> points;
    const Point origin{0, 0, 0};
    points.push_back(origin);
    points.push_back(origin);

    std::vector<IndexAndSquaredDistance<Point> > result;
    TestKNearest_NoIndex(points, result);
    ASSERT_EQ(2, result.size());
}

TEST(KNearestNeighbor, MoreThanKPoints) {
    std::vector<Point> points;
    const Point origin{0, 0, 0};
    points.push_back(origin);
    points.push_back(origin);
    points.push_back(origin);
    points.push_back(origin);

    std::vector<IndexAndSquaredDistance<Point> > result;
    TestKNearest_NoIndex(points, result);
    ASSERT_EQ(2, result.size());

    ASSERT_EQ(2, result.size());
}

TEST(KNearestNeighbor, GenericCase) {
    std::vector<Point> points;
    const Point referencePoint{0, 0, 0};
    points.push_back(Point{70, 1, 2});
    points.push_back(Point{11, 1, 2});
    points.push_back(Point{7, 1, 2});
    points.push_back(Point{7, 2, 2});
    points.push_back(Point{7, 2, -225});

    NoIndex<Point> geometryIndex;
    BuildIndex(points, geometryIndex);

    const PointTraits<Point>::coordinate cullingDistance = 65;

    const size_t k = 3;
    std::vector<IndexAndSquaredDistance<Point> > result;

    KNearestNeighbor(
        geometryIndex,
        cullingDistance,
        referencePoint,
        k,
        result);

    ASSERT_EQ(3, result.size());
    ASSERT_INDEX_PRESENT(result, 2);
    ASSERT_INDEX_PRESENT(result, 3);
    ASSERT_INDEX_PRESENT(result, 1);
}

#ifdef GEO_INDEX_SAFETY_CHECKS

TEST(KNearestNeighbor, zeroDistance) {
    const std::vector<Point> points;
    const NoIndex<Point> geometryIndex;
    const PointTraits<Point>::coordinate cullingDistance = 0;

    const Point referencePoint{0, 0, 0};
    const size_t k = 4;
    std::vector<IndexAndSquaredDistance<Point> > result;

    ASSERT_ANY_THROW(
        KNearestNeighbor(
            geometryIndex,
            cullingDistance,
            referencePoint,
            k,
            result)
    );
}

TEST(KNearestNeighbor, zeroK) {
    const std::vector<Point> points;
    const NoIndex<Point> geometryIndex;
    const PointTraits<Point>::coordinate cullingDistance = 10;

    const Point referencePoint{0, 0, 0};
    const size_t k = 0;
    std::vector<IndexAndSquaredDistance<Point> > result;

    ASSERT_ANY_THROW(
        KNearestNeighbor(
            geometryIndex,
            cullingDistance,
            referencePoint,
            k,
            result)
    );
}

#endif
/** This class makes little sense. It is just to show how to use custom classes. */
class PointAndClick {
public:
    PointAndClick() :
        x(0), y(0), z(0)
        {};
    int x;
    int y;
    int z;
    void doNothing(){};
private:
    char whyNotACharHere;
    float someValue;
};

template<>
struct PointTraits<PointAndClick> {
    typedef int coordinate;
    typedef unsigned int index;
};

TEST(KNearestNeighbor, UserDefinedClasses) {
    NoIndex<PointAndClick> geometryIndex;
    std::vector<PointAndClick> points;
    BuildIndex(points, geometryIndex);
    PointAndClick referencePoint;

    const size_t k = 2;

    std::vector<IndexAndSquaredDistance<PointAndClick> > result;

    KNearestNeighbor(
        geometryIndex,
        2,
        referencePoint,
        k,
        result);
}

}
<|endoftext|>


<commit_before>#include "AConfig.h"
#if(HAS_STD_2X1_THRUSTERS)
#include "Device.h"
#include "Pin.h"
#include "Thrusters2X1.h"
#include "Settings.h"
#include "Motor.h"
#include "Timer.h"

//Motors motors(9, 10, 11);
//Motors motors(6, 7, 8);
Motor port_motor(PORT_PIN);
Motor vertical_motor(VERTICAL_PIN);
Motor starboard_motor(STARBOARD_PIN);

int new_p = MIDPOINT;
int new_s = MIDPOINT;
int new_v = MIDPOINT;
int p = MIDPOINT;
int v = MIDPOINT;
int s = MIDPOINT;

float trg_throttle,trg_yaw,trg_lift;
int trg_motor_power;

Timer controltime;
Timer thrusterOutput;
boolean bypasssmoothing;

#ifdef ESCPOWER_PIN
bool canPowerESCs = true;
Pin escpower("escpower", ESCPOWER_PIN, escpower.digital, escpower.out);
#else
boolean canPowerESCs = false;
#endif

int smoothAdjustedServoPosition(int target, int current){
  // if the MIDPOINT is betwen the change requested in velocity we want to go to MIDPOINT first, and right away.
  if (((current < MIDPOINT) && (MIDPOINT < target)) || ((target < MIDPOINT) && (MIDPOINT < current))){
    return MIDPOINT;
  }
  // if the change is moving us closer to MIDPOINT it is a reduction of power and we can move all the way to the target
  // in one command
  if (abs(MIDPOINT-target) < abs(MIDPOINT-current)){
    return target;
  }
  // else, we need to smooth out amp spikes by making a series of incrimental changes in the motors, so only move part of
  // the way to the target this time.
  double x = target - current;
  int sign = (x>0) - (x<0);
  int adjustedVal = current + sign * (min(abs(target - current), Settings::smoothingIncriment));
  // skip the deadzone
  if (sign<0) {
    return (min(adjustedVal,Settings::deadZone_min));
  } else if(sign>0){
    return (max(adjustedVal,Settings::deadZone_max));
  } else
    return (adjustedVal);
}

void Thrusters::device_setup(){
  port_motor.reset();
  vertical_motor.reset();
  starboard_motor.reset();
  thrusterOutput.reset();
  controltime.reset();
  bypasssmoothing = false;
  #ifdef ESCPOWER_PIN
    escpower.reset();
    escpower.write(1); //Turn on the ESCs

  #endif
}

void Thrusters::device_loop(Command command){
  if (command.cmp("mtrmod")) {
      port_motor.motor_positive_modifer = command.args[1]/100;
      vertical_motor.motor_positive_modifer = command.args[2]/100;
      starboard_motor.motor_positive_modifer = command.args[3]/100;
      port_motor.motor_negative_modifer = command.args[4]/100;
      vertical_motor.motor_negative_modifer = command.args[5]/100;
      starboard_motor.motor_negative_modifer = command.args[6]/100;
  }
  if (command.cmp("rmtrmod")) {
      Serial.print(F("mtrmod:"));
      Serial.print(port_motor.motor_positive_modifer);
      Serial.print (",");
      Serial.print(vertical_motor.motor_positive_modifer);
      Serial.print (",");
      Serial.print(starboard_motor.motor_positive_modifer);
      Serial.print (",");
      Serial.print(port_motor.motor_negative_modifer);
      Serial.print (",");
      Serial.print(vertical_motor.motor_negative_modifer);
      Serial.print (",");
      Serial.print(starboard_motor.motor_negative_modifer);
      Serial.println (";");
  }

  if (command.cmp("go")) {
      //ignore corrupt data
      if (command.args[1]>999 && command.args[2] >999 && command.args[3] > 999 && command.args[1]<2001 && command.args[2]<2001 && command.args[3] < 2001) {
        p = command.args[1];
        v = command.args[2];
        s = command.args[3];
        if (command.args[4] == 1) bypasssmoothing=true;
      }
    }

  if (command.cmp("port")) {
      //ignore corrupt data
      if (command.args[1]>999 && command.args[1]<2001) {
        p = command.args[1];
        if (command.args[2] == 1) bypasssmoothing=true;
      }
  }

  if (command.cmp("vertical")) {
      //ignore corrupt data
      if (command.args[1]>999 && command.args[1]<2001) {
        v = command.args[1];
        if (command.args[2] == 1) bypasssmoothing=true;
      }
  }

  if (command.cmp("starboard")) {
      //ignore corrupt data
      if (command.args[1]>999 && command.args[1]<2001) {
        s = command.args[1];
        if (command.args[2] == 1) bypasssmoothing=true;
      }
  }

  if (command.cmp("thro") || command.cmp("yaw")){
    if (command.cmp("thro")){
      if (command.args[1]>=-100 && command.args[1]<=100) {
        trg_throttle = command.args[1]/100.0;
      }
    }

    if (command.cmp("yaw")) {
        //ignore corrupt data
        if (command.args[1]>=-100 && command.args[1]<=100) { //percent of max turn
          trg_yaw = command.args[1]/100.0;
        }
    }

    // The code below spreads the throttle spectrum over the possible range
    // of the motor. Not sure this belongs here or should be placed with
    // deadzon calculation in the motor code.
    if (trg_throttle>=0){
      p = 1500 + (500/abs(port_motor.motor_positive_modifer))*trg_throttle;
      s = p;
    } else {
      p = 1500 + (500/abs(port_motor.motor_negative_modifer))*trg_throttle;
      s = p;
    }

    trg_motor_power = s;

    int turn = trg_yaw*250; //max range due to reverse range
    if (trg_throttle >=0){
      int offset = (abs(turn)+trg_motor_power)-2000;
      if (offset < 0) offset = 0;
      p = trg_motor_power+turn-offset;
      s = trg_motor_power-turn-offset;
    } else {
      int offset = 1000-(trg_motor_power-abs(turn));
      if (offset < 0) offset = 0;
      p = trg_motor_power+turn+offset;
      s = trg_motor_power-turn+offset;
    }

  }

  if (command.cmp("lift")){
    if (command.args[1]>=-100 && command.args[1]<=100) {
      trg_lift = command.args[1]/100.0;
      v = 1500 + 500*trg_lift;
    }
  }

  #ifdef ESCPOWER_PIN
    else if (command.cmp("escp")) {
      escpower.write(command.args[1]); //Turn on the ESCs
      Serial.print(F("log:escpower="));
      Serial.print(command.args[1]);
      Serial.println(';');
    }
  #endif
    else if (command.cmp("start")) {
      port_motor.reset();
      vertical_motor.reset();
      starboard_motor.reset();
    }
    else if (command.cmp("stop")) {
      port_motor.stop();
      vertical_motor.reset();
      starboard_motor.reset();
    }
    #ifdef ESCPOWER_PIN
    else if ((command.cmp("mcal")) && (canPowerESCs)){
      Serial.println(F("log:Motor Callibration Staring;"));
      //Experimental. Add calibration code here
      Serial.println(F("log:Motor Callibration Complete;"));
  }
    #endif
  //to reduce AMP spikes, smooth large power adjustments out. This incirmentally adjusts the motors and servo
  //to their new positions in increments.  The incriment should eventually be adjustable from the cockpit so that
  //the pilot could have more aggressive response profiles for the ROV.
  if (controltime.elapsed (50)) {
    if (p!=new_p || v!=new_v || s!=new_s) {
      new_p = smoothAdjustedServoPosition(p,new_p);
      new_v = smoothAdjustedServoPosition(v,new_v);
      new_s = smoothAdjustedServoPosition(s,new_s);
      if (bypasssmoothing)
      {
        new_p=p;
        new_v=v;
        new_s=s;
        bypasssmoothing = false;
      }
      Serial.print(F("motors:"));
      Serial.print(port_motor.goms(new_p));
      Serial.print(',');
      Serial.print(vertical_motor.goms(new_v));
      Serial.print(',');
      Serial.print(starboard_motor.goms(new_s));
      Serial.println(';');
    }

  }

  navdata::FTHR = map((new_p + new_s) / 2, 1000,2000,-100,100);

  //The output from the motors is unique to the thruster configuration
  if (thrusterOutput.elapsed(1000)){
    Serial.print(F("mtarg:"));
    Serial.print(p);
    Serial.print(',');
    Serial.print(v);
    Serial.print(',');
    Serial.print(s);
    Serial.println(';');

  }
}
#endif
<commit_msg>Fix stop command on port motor<commit_after>#include "AConfig.h"
#if(HAS_STD_2X1_THRUSTERS)
#include "Device.h"
#include "Pin.h"
#include "Thrusters2X1.h"
#include "Settings.h"
#include "Motor.h"
#include "Timer.h"

//Motors motors(9, 10, 11);
//Motors motors(6, 7, 8);
Motor port_motor(PORT_PIN);
Motor vertical_motor(VERTICAL_PIN);
Motor starboard_motor(STARBOARD_PIN);

int new_p = MIDPOINT;
int new_s = MIDPOINT;
int new_v = MIDPOINT;
int p = MIDPOINT;
int v = MIDPOINT;
int s = MIDPOINT;

float trg_throttle,trg_yaw,trg_lift;
int trg_motor_power;

Timer controltime;
Timer thrusterOutput;
boolean bypasssmoothing;

#ifdef ESCPOWER_PIN
bool canPowerESCs = true;
Pin escpower("escpower", ESCPOWER_PIN, escpower.digital, escpower.out);
#else
boolean canPowerESCs = false;
#endif

int smoothAdjustedServoPosition(int target, int current){
  // if the MIDPOINT is betwen the change requested in velocity we want to go to MIDPOINT first, and right away.
  if (((current < MIDPOINT) && (MIDPOINT < target)) || ((target < MIDPOINT) && (MIDPOINT < current))){
    return MIDPOINT;
  }
  // if the change is moving us closer to MIDPOINT it is a reduction of power and we can move all the way to the target
  // in one command
  if (abs(MIDPOINT-target) < abs(MIDPOINT-current)){
    return target;
  }
  // else, we need to smooth out amp spikes by making a series of incrimental changes in the motors, so only move part of
  // the way to the target this time.
  double x = target - current;
  int sign = (x>0) - (x<0);
  int adjustedVal = current + sign * (min(abs(target - current), Settings::smoothingIncriment));
  // skip the deadzone
  if (sign<0) {
    return (min(adjustedVal,Settings::deadZone_min));
  } else if(sign>0){
    return (max(adjustedVal,Settings::deadZone_max));
  } else
    return (adjustedVal);
}

void Thrusters::device_setup(){
  port_motor.reset();
  vertical_motor.reset();
  starboard_motor.reset();
  thrusterOutput.reset();
  controltime.reset();
  bypasssmoothing = false;
  #ifdef ESCPOWER_PIN
    escpower.reset();
    escpower.write(1); //Turn on the ESCs
  #endif
}

void Thrusters::device_loop(Command command){
  if (command.cmp("mtrmod")) {
      port_motor.motor_positive_modifer = command.args[1]/100;
      vertical_motor.motor_positive_modifer = command.args[2]/100;
      starboard_motor.motor_positive_modifer = command.args[3]/100;
      port_motor.motor_negative_modifer = command.args[4]/100;
      vertical_motor.motor_negative_modifer = command.args[5]/100;
      starboard_motor.motor_negative_modifer = command.args[6]/100;
  }
  if (command.cmp("rmtrmod")) {
      Serial.print(F("mtrmod:"));
      Serial.print(port_motor.motor_positive_modifer);
      Serial.print (",");
      Serial.print(vertical_motor.motor_positive_modifer);
      Serial.print (",");
      Serial.print(starboard_motor.motor_positive_modifer);
      Serial.print (",");
      Serial.print(port_motor.motor_negative_modifer);
      Serial.print (",");
      Serial.print(vertical_motor.motor_negative_modifer);
      Serial.print (",");
      Serial.print(starboard_motor.motor_negative_modifer);
      Serial.println (";");
  }

  if (command.cmp("go")) {
      //ignore corrupt data
      if (command.args[1]>999 && command.args[2] >999 && command.args[3] > 999 && command.args[1]<2001 && command.args[2]<2001 && command.args[3] < 2001) {
        p = command.args[1];
        v = command.args[2];
        s = command.args[3];
        if (command.args[4] == 1) bypasssmoothing=true;
      }
    }

  if (command.cmp("port")) {
      //ignore corrupt data
      if (command.args[1]>999 && command.args[1]<2001) {
        p = command.args[1];
        if (command.args[2] == 1) bypasssmoothing=true;
      }
  }

  if (command.cmp("vertical")) {
      //ignore corrupt data
      if (command.args[1]>999 && command.args[1]<2001) {
        v = command.args[1];
        if (command.args[2] == 1) bypasssmoothing=true;
      }
  }

  if (command.cmp("starboard")) {
      //ignore corrupt data
      if (command.args[1]>999 && command.args[1]<2001) {
        s = command.args[1];
        if (command.args[2] == 1) bypasssmoothing=true;
      }
  }

  if (command.cmp("thro") || command.cmp("yaw")){
    if (command.cmp("thro")){
      if (command.args[1]>=-100 && command.args[1]<=100) {
        trg_throttle = command.args[1]/100.0;
      }
    }

    if (command.cmp("yaw")) {
        //ignore corrupt data
        if (command.args[1]>=-100 && command.args[1]<=100) { //percent of max turn
          trg_yaw = command.args[1]/100.0;
        }
    }

    // The code below spreads the throttle spectrum over the possible range
    // of the motor. Not sure this belongs here or should be placed with
    // deadzon calculation in the motor code.
    if (trg_throttle>=0){
      p = 1500 + (500/abs(port_motor.motor_positive_modifer))*trg_throttle;
      s = p;
    } else {
      p = 1500 + (500/abs(port_motor.motor_negative_modifer))*trg_throttle;
      s = p;
    }

    trg_motor_power = s;

    int turn = trg_yaw*250; //max range due to reverse range
    if (trg_throttle >=0){
      int offset = (abs(turn)+trg_motor_power)-2000;
      if (offset < 0) offset = 0;
      p = trg_motor_power+turn-offset;
      s = trg_motor_power-turn-offset;
    } else {
      int offset = 1000-(trg_motor_power-abs(turn));
      if (offset < 0) offset = 0;
      p = trg_motor_power+turn+offset;
      s = trg_motor_power-turn+offset;
    }

  }

  if (command.cmp("lift")){
    if (command.args[1]>=-100 && command.args[1]<=100) {
      trg_lift = command.args[1]/100.0;
      v = 1500 + 500*trg_lift;
    }
  }

  #ifdef ESCPOWER_PIN
    else if (command.cmp("escp")) {
      escpower.write(command.args[1]); //Turn on the ESCs
      Serial.print(F("log:escpower="));
      Serial.print(command.args[1]);
      Serial.println(';');
    }
  #endif
    else if (command.cmp("start")) {
      port_motor.reset();
      vertical_motor.reset();
      starboard_motor.reset();
    }
    else if (command.cmp("stop")) {
      port_motor.reset();
      vertical_motor.reset();
      starboard_motor.reset();
    }
    #ifdef ESCPOWER_PIN
    else if ((command.cmp("mcal")) && (canPowerESCs)){
      Serial.println(F("log:Motor Callibration Staring;"));
      //Experimental. Add calibration code here
      Serial.println(F("log:Motor Callibration Complete;"));
  }
    #endif
  //to reduce AMP spikes, smooth large power adjustments out. This incirmentally adjusts the motors and servo
  //to their new positions in increments.  The incriment should eventually be adjustable from the cockpit so that
  //the pilot could have more aggressive response profiles for the ROV.
  if (controltime.elapsed (50)) {
    if (p!=new_p || v!=new_v || s!=new_s) {
      new_p = smoothAdjustedServoPosition(p,new_p);
      new_v = smoothAdjustedServoPosition(v,new_v);
      new_s = smoothAdjustedServoPosition(s,new_s);
      if (bypasssmoothing)
      {
        new_p=p;
        new_v=v;
        new_s=s;
        bypasssmoothing = false;
      }
      Serial.print(F("motors:"));
      Serial.print(port_motor.goms(new_p));
      Serial.print(',');
      Serial.print(vertical_motor.goms(new_v));
      Serial.print(',');
      Serial.print(starboard_motor.goms(new_s));
      Serial.println(';');
    }

  }

  navdata::FTHR = map((new_p + new_s) / 2, 1000,2000,-100,100);

  //The output from the motors is unique to the thruster configuration
  if (thrusterOutput.elapsed(1000)){
    Serial.print(F("mtarg:"));
    Serial.print(p);
    Serial.print(',');
    Serial.print(v);
    Serial.print(',');
    Serial.print(s);
    Serial.println(';');

  }
}
#endif
<|endoftext|>


<commit_before>#include "Menu_state.h"
#include "Play_state.h"
#include "Menu_button.h"
#include "utils.h"
#include "Game.h"
#include "Texture_manager.h"

const std::string Menu_state::s_menu_id = "MENU";

void Menu_state::update() {
	for (std::vector<Game_object*>::size_type i = 0; i < m_game_objects.size(); i++) {
		m_game_objects[i]->update();
	}
}
void Menu_state::render() {
	for (std::vector<Game_object*>::size_type i = 0; i < m_game_objects.size(); i++) {
		m_game_objects[i]->draw();
	}
}

bool Menu_state::on_enter() {
	LOG << "entering Menu_state";

	if (!The_Texture_manager->load("../assets/btn_start.png", "btn_start", The_Game->get_renderer()))
		return false;
	if (!The_Texture_manager->load("../assets/btn_exit.png", "btn_exit", The_Game->get_renderer()))
		return false;

	m_game_objects.push_back(new Menu_button(new Loader_params(100, 100, 150, 50, "btn_start"), menu_to_play));
	m_game_objects.push_back(new Menu_button(new Loader_params(100, 155, 150, 50, "btn_exit"), menu_to_exit));

	return true;
}
bool Menu_state::on_exit() {
	LOG << "exiting Menu_state";

	for (std::vector<Game_object*>::const_iterator i = m_game_objects.begin(); i != m_game_objects.end(); i++) {
		(*i)->clean();
	}
	m_game_objects.clear();
	The_Texture_manager->clear_from_texture_map("btn_start");
	The_Texture_manager->clear_from_texture_map("btn_exit");

	return true;
}

void Menu_state::menu_to_play() {
	The_Game->get_state_machine()->change_state(new Play_state());
	LOG << "Play btn pressed";
}

void Menu_state::menu_to_exit() {
	LOG << "Exit btn pressed";
	The_Game->quit();
}
<commit_msg>debug logging<commit_after>#include "Menu_state.h"
#include "Play_state.h"
#include "Menu_button.h"
#include "utils.h"
#include "Game.h"
#include "Texture_manager.h"

const std::string Menu_state::s_menu_id = "MENU";

void Menu_state::update() {
	for (std::vector<Game_object*>::size_type i = 0; i<m_game_objects.size() ; i++) {
		LOG << "now im gonna kill myself" << i << m_game_objects.size();
		m_game_objects[i]->update();
		LOG << "im done thank u!!!4444" << i << m_game_objects.size();
	}
}
void Menu_state::render() {
	for (std::vector<Game_object*>::size_type i = 0; i < m_game_objects.size(); i++) {
		m_game_objects[i]->draw();
	}
}

bool Menu_state::on_enter() {
	LOG << "entering Menu_state";

	if (!The_Texture_manager->load("../assets/btn_start.png", "btn_start", The_Game->get_renderer()))
		return false;
	if (!The_Texture_manager->load("../assets/btn_exit.png", "btn_exit", The_Game->get_renderer()))
		return false;

	m_game_objects.push_back(new Menu_button(new Loader_params(100, 100, 150, 50, "btn_start"), menu_to_play));
	m_game_objects.push_back(new Menu_button(new Loader_params(100, 155, 150, 50, "btn_exit"), menu_to_exit));

	return true;
}
bool Menu_state::on_exit() {
	LOG << "exiting Menu_state";

	for (std::vector<Game_object*>::size_type i = 0; i < m_game_objects.size(); i++)
		m_game_objects[i]->clean();

	m_game_objects.clear();
	The_Texture_manager->clear_from_texture_map("btn_start");
	The_Texture_manager->clear_from_texture_map("btn_exit");

	return true;
}

void Menu_state::menu_to_play() {
	LOG << "Play btn pressed";
	The_Game->get_state_machine()->change_state(new Play_state());
	LOG << "fasz1";
}

void Menu_state::menu_to_exit() {
	LOG << "Exit btn pressed";
	The_Game->get_state_machine()->pop_state();
	The_Game->quit();
}
<|endoftext|>


<commit_before>/* InverseDynamicsSolver.cpp
* Author: Ajay Seth
* Copyright (c)  2006 Stanford University
* Use of the OpenSim software in source form is permitted provided that the following
* conditions are met:
* 	1. The software is used only for non-commercial research and education. It may not
*     be used in relation to any commercial activity.
* 	2. The software is not distributed or redistributed.  Software distribution is allowed
*     only through https://simtk.org/home/opensim.
* 	3. Use of the OpenSim software or derivatives must be acknowledged in all publications,
*      presentations, or documents describing work in which OpenSim or derivatives are used.
* 	4. Credits to developers may not be removed from executables
*     created from modifications of the source.
* 	5. Modifications of source code must retain the above copyright notice, this list of
*     conditions and the following disclaimer.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
*  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
*  OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
*  SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
*  TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
*  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
*  OR BUSINESS INTERRUPTION) OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
*  WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "InverseDynamicsSolver.h"
#include "Model/Model.h"
#include <OpenSim/Common/FunctionSet.h>

using namespace std;
using namespace SimTK;

namespace OpenSim {

//______________________________________________________________________________
/**
 * An implementation of the InverseDynamicsSolver
 *
 * @param model to assemble
 */
	InverseDynamicsSolver::InverseDynamicsSolver(const Model &model) : Solver(model)
{
}

/** Solve the inverse dynamics system of equations for generalized coordinate forces, Tau.
    Applied loads are computed by the model from the state.	 */
Vector InverseDynamicsSolver::solve(SimTK::State &s, const SimTK::Vector &udot)
{
	// Default is a statics inverse dynamics analysis, udot = 0;
	Vector knownUdots(_model.getNumSpeeds(), 0.0);

	// If valid accelerations provided then use for inverse dynamics
	if(udot.size() == _model.getNumSpeeds()){
		knownUdots = udot;
	}
	else if (udot.size() != 0){
		throw Exception("InverseDynamicsSolver::solve with input 'udot' of invalid size.");
	}

	// Realize to dynamics stage so that all model forces are computed
	_model.getMultibodySystem().realize(s,SimTK::Stage::Dynamics);

	// Get applied mobility (generalized) forces generated by components of the model, like actuators
	const Vector &appliedMobilityForces = _model.getMultibodySystem().getMobilityForces(s, Stage::Dynamics);

	// Get all applied body forces like those from conact
	const Vector_<SpatialVec>& appliedBodyForces = _model.getMultibodySystem().getRigidBodyForces(s, Stage::Dynamics);

	// Perform general inverse dynamics
	return solve(s,	knownUdots, appliedMobilityForces, appliedBodyForces);
}

/** Solve the inverse dynamics system of equations for generalized coordinate forces, Tau.
    Applied loads are explicity provided as generalized coordinate forces (MobilityForces)
	and/or a Vector of Spatial-body forces */
Vector InverseDynamicsSolver::solve(const SimTK::State &s, const SimTK::Vector &udot,
	const SimTK::Vector &appliedMobilityForces, const SimTK::Vector_<SimTK::SpatialVec>& appliedBodyForces)
{
	//Results of the inverse dynamics for the generalized forces to satisfy accelerations
	Vector residualMobilityForces;

	if(s.getSystemStage() < SimTK::Stage::Dynamics)
		_model.getMultibodySystem().realize(s,SimTK::Stage::Dynamics);

	// Perform inverse dynamics
	_model.getMultibodySystem().getMatterSubsystem().calcResidualForceIgnoringConstraints(s,
			appliedMobilityForces, appliedBodyForces, udot, residualMobilityForces);

	return residualMobilityForces;
}

/** Solve the inverse dynamics system of equations for generalized coordinate forces, Tau.
    Now the state is not given, but is constructed from known coordinates, q as functions of time.
	Coordinate functions must be twice differentiable.
	NOTE: state dependent forces and other applied loads are NOT computed since these may depend on
	state variables (like muscle fiber lengths) that are not known */
Vector InverseDynamicsSolver::solve(State &s, const FunctionSet &Qs, double time)
{
	int nq = _model.getNumCoordinates();

	if(Qs.getSize() != nq){
		throw Exception("InverseDynamicsSolver::solve invalid number of q functions.");
	}

	if( nq != _model.getNumSpeeds()){
		throw Exception("InverseDynamicsSolver::solve using FunctionSet, nq != nu not supported.");
	}

	// update the State so we get the correct gravity and coriolis effects
	// direct references into the state so no allocation required
	s.updTime() = time;
	Vector &q = s.updQ();
	Vector &u = s.updU();
	Vector &udot = s.updUDot();

	for(int i=0; i<nq; i++){
		q[i] = Qs.evaluate(i, 0, time);
		u[i] = Qs.evaluate(i, 1, time);
		udot[i] = Qs.evaluate(i, 2, time);
	}

	// Perform general inverse dynamics
	return solve(s,	udot);
}

/** Same as above but for a given time series */
void InverseDynamicsSolver::solve(State &s, const FunctionSet &Qs, const Array_<double> &times, Array_<Vector> &genForceTrajectory)
{
	int nq = _model.getNumCoordinates();
	int nt = times.size();

	//Preallocate if not done already
	genForceTrajectory.resize(nt, Vector(nq));

	//fill in results for each time
	for(int i=0; i<nt; i++){
		genForceTrajectory[i] = solve(s, Qs, times[i]);
	}
}

} // end of namespace OpenSim<commit_msg>Enable callbacks while solving InverseDynamics to update display<commit_after>/* InverseDynamicsSolver.cpp
* Author: Ajay Seth
* Copyright (c)  2006 Stanford University
* Use of the OpenSim software in source form is permitted provided that the following
* conditions are met:
* 	1. The software is used only for non-commercial research and education. It may not
*     be used in relation to any commercial activity.
* 	2. The software is not distributed or redistributed.  Software distribution is allowed
*     only through https://simtk.org/home/opensim.
* 	3. Use of the OpenSim software or derivatives must be acknowledged in all publications,
*      presentations, or documents describing work in which OpenSim or derivatives are used.
* 	4. Credits to developers may not be removed from executables
*     created from modifications of the source.
* 	5. Modifications of source code must retain the above copyright notice, this list of
*     conditions and the following disclaimer.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
*  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
*  OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
*  SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
*  TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
*  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
*  OR BUSINESS INTERRUPTION) OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
*  WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "InverseDynamicsSolver.h"
#include "Model/Model.h"
#include <OpenSim/Common/FunctionSet.h>

using namespace std;
using namespace SimTK;

namespace OpenSim {

//______________________________________________________________________________
/**
 * An implementation of the InverseDynamicsSolver
 *
 * @param model to assemble
 */
	InverseDynamicsSolver::InverseDynamicsSolver(const Model &model) : Solver(model)
{
}

/** Solve the inverse dynamics system of equations for generalized coordinate forces, Tau.
    Applied loads are computed by the model from the state.	 */
Vector InverseDynamicsSolver::solve(SimTK::State &s, const SimTK::Vector &udot)
{
	// Default is a statics inverse dynamics analysis, udot = 0;
	Vector knownUdots(_model.getNumSpeeds(), 0.0);

	// If valid accelerations provided then use for inverse dynamics
	if(udot.size() == _model.getNumSpeeds()){
		knownUdots = udot;
	}
	else if (udot.size() != 0){
		throw Exception("InverseDynamicsSolver::solve with input 'udot' of invalid size.");
	}

	// Realize to dynamics stage so that all model forces are computed
	_model.getMultibodySystem().realize(s,SimTK::Stage::Dynamics);

	// Get applied mobility (generalized) forces generated by components of the model, like actuators
	const Vector &appliedMobilityForces = _model.getMultibodySystem().getMobilityForces(s, Stage::Dynamics);

	// Get all applied body forces like those from conact
	const Vector_<SpatialVec>& appliedBodyForces = _model.getMultibodySystem().getRigidBodyForces(s, Stage::Dynamics);

	// Perform general inverse dynamics
	return solve(s,	knownUdots, appliedMobilityForces, appliedBodyForces);
}

/** Solve the inverse dynamics system of equations for generalized coordinate forces, Tau.
    Applied loads are explicity provided as generalized coordinate forces (MobilityForces)
	and/or a Vector of Spatial-body forces */
Vector InverseDynamicsSolver::solve(const SimTK::State &s, const SimTK::Vector &udot,
	const SimTK::Vector &appliedMobilityForces, const SimTK::Vector_<SimTK::SpatialVec>& appliedBodyForces)
{
	//Results of the inverse dynamics for the generalized forces to satisfy accelerations
	Vector residualMobilityForces;

	if(s.getSystemStage() < SimTK::Stage::Dynamics)
		_model.getMultibodySystem().realize(s,SimTK::Stage::Dynamics);

	// Perform inverse dynamics
	_model.getMultibodySystem().getMatterSubsystem().calcResidualForceIgnoringConstraints(s,
			appliedMobilityForces, appliedBodyForces, udot, residualMobilityForces);

	return residualMobilityForces;
}

/** Solve the inverse dynamics system of equations for generalized coordinate forces, Tau.
    Now the state is not given, but is constructed from known coordinates, q as functions of time.
	Coordinate functions must be twice differentiable.
	NOTE: state dependent forces and other applied loads are NOT computed since these may depend on
	state variables (like muscle fiber lengths) that are not known */
Vector InverseDynamicsSolver::solve(State &s, const FunctionSet &Qs, double time)
{
	int nq = _model.getNumCoordinates();

	if(Qs.getSize() != nq){
		throw Exception("InverseDynamicsSolver::solve invalid number of q functions.");
	}

	if( nq != _model.getNumSpeeds()){
		throw Exception("InverseDynamicsSolver::solve using FunctionSet, nq != nu not supported.");
	}

	// update the State so we get the correct gravity and coriolis effects
	// direct references into the state so no allocation required
	s.updTime() = time;
	Vector &q = s.updQ();
	Vector &u = s.updU();
	Vector &udot = s.updUDot();

	for(int i=0; i<nq; i++){
		q[i] = Qs.evaluate(i, 0, time);
		u[i] = Qs.evaluate(i, 1, time);
		udot[i] = Qs.evaluate(i, 2, time);
	}

	// Perform general inverse dynamics
	return solve(s,	udot);
}

/** Same as above but for a given time series */
void InverseDynamicsSolver::solve(State &s, const FunctionSet &Qs, const Array_<double> &times, Array_<Vector> &genForceTrajectory)
{
	int nq = _model.getNumCoordinates();
	int nt = times.size();

	//Preallocate if not done already
	genForceTrajectory.resize(nt, Vector(nq));

	AnalysisSet& analysisSet = const_cast<AnalysisSet&>(_model.getAnalysisSet());
	//fill in results for each time
	for(int i=0; i<nt; i++){
		genForceTrajectory[i] = solve(s, Qs, times[i]);
		analysisSet.step(s, i);
	}
}

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


<commit_before>/*===- RectConfinementForce.cpp - libSimulation -===============================
*
*                                  DEMON
*
* This file is distributed under the BSD Open Source License. See LICENSE.TXT
* for details.
*
*===-----------------------------------------------------------------------===*/

#include "RectConfinementForce.h"

RectConfinementForce::RectConfinementForce(Cloud * const myCloud, double confineConstX, double confineConstY)
: Force(myCloud), confineX(-confineConstX), confineY(-confineConstY) {}

void RectConfinementForce::force1(const double currentTime)
{
	for (unsigned int currentParticle = 0, numParticles = cloud->n; currentParticle < numParticles; currentParticle += 2)
		force(currentParticle, cloud->getx1_pd(currentParticle), cloud->gety1_pd(currentParticle));
}

void RectConfinementForce::force2(const double currentTime)
{
	for (unsigned int currentParticle = 0, numParticles = cloud->n; currentParticle < numParticles; currentParticle += 2)
		force(currentParticle, cloud->getx2_pd(currentParticle), cloud->gety2_pd(currentParticle));
}

void RectConfinementForce::force3(const double currentTime)
{
	for (unsigned int currentParticle = 0, numParticles = cloud->n; currentParticle < numParticles; currentParticle += 2)
		force(currentParticle, cloud->getx3_pd(currentParticle), cloud->gety3_pd(currentParticle));
}

void RectConfinementForce::force4(const double currentTime)
{
	for (unsigned int currentParticle = 0, numParticles = cloud->n; currentParticle < numParticles; currentParticle += 2)
		force(currentParticle, cloud->getx4_pd(currentParticle), cloud->gety4_pd(currentParticle));
}

inline void RectConfinementForce::force(const unsigned int currentParticle, const __m128d currentPositionX, const __m128d currentPositionY)
{
	const __m128d cVx = _mm_set1_pd(confineX);
	const __m128d cVy = _mm_set1_pd(confineY);
	double * const pFx = cloud->forceX + currentParticle;
	double * const pFy = cloud->forceY + currentParticle;

	_mm_store_pd(pFx, _mm_load_pd(pFx) + _mm_mul_pd(cVx, currentPositionX));
	_mm_store_pd(pFy, _mm_load_pd(pFy) + _mm_mul_pd(cVy, currentPositionY));
}

void RectConfinementForce::writeForce(fitsfile * const file, int * const error) const
{
	//move to primary HDU:
	if(!*error)
		//file, # indicating primary HDU, HDU type, error
 		fits_movabs_hdu(file, 1, IMAGE_HDU, error);

	//add flag indicating that the rectangular confinement force is used:
	if(!*error)
	{
		long forceFlags = 0;
		fits_read_key_lng(file, const_cast<char *> ("FORCES"), &forceFlags, NULL, error);

		//add RectConfinementForce bit:
		forceFlags |= RectConfinementForceFlag;		//compound bitwise OR

		if(*error == KEY_NO_EXIST || *error == VALUE_UNDEFINED)
			*error = 0;			//clear above error.

		//add or update keyword:
		if(!*error)
			fits_update_key(file, TLONG, const_cast<char *> ("FORCES"), &forceFlags, const_cast<char *> ("Force configuration."), error);
	}

	if(!*error)
	{
		//file, key name, value, precision (scientific format), comment
		fits_write_key_dbl(file, const_cast<char *> ("confineConstX"), confineX, 6, const_cast<char *> ("[N/m] (RectConfinementForce)"), error);
		fits_write_key_dbl(file, const_cast<char *> ("confineConstY"), confineY, 6, const_cast<char *> ("[N/m] (RectConfinementForce)"), error);
	}
}

void RectConfinementForce::readForce(fitsfile * const file, int * const error)
{
	//move to primary HDU:
	if(!*error)
		//file, # indicating primary HDU, HDU type, error
 		fits_movabs_hdu(file, 1, IMAGE_HDU, error);

	if(!*error)
	{
		//file, key name, value, don't read comment, error
		fits_read_key_dbl(file, const_cast<char *> ("confineConstX"), &confineX, NULL, error);
		fits_read_key_dbl(file, const_cast<char *> ("confineConstY"), &confineY, NULL, error);
	}
}
<commit_msg>Inline constants since they were only being used once in RectConfinementForce.<commit_after>/*===- RectConfinementForce.cpp - libSimulation -===============================
*
*                                  DEMON
*
* This file is distributed under the BSD Open Source License. See LICENSE.TXT
* for details.
*
*===-----------------------------------------------------------------------===*/

#include "RectConfinementForce.h"

RectConfinementForce::RectConfinementForce(Cloud * const myCloud, double confineConstX, double confineConstY)
: Force(myCloud), confineX(-confineConstX), confineY(-confineConstY) {}

void RectConfinementForce::force1(const double currentTime)
{
	for (unsigned int currentParticle = 0, numParticles = cloud->n; currentParticle < numParticles; currentParticle += 2)
		force(currentParticle, cloud->getx1_pd(currentParticle), cloud->gety1_pd(currentParticle));
}

void RectConfinementForce::force2(const double currentTime)
{
	for (unsigned int currentParticle = 0, numParticles = cloud->n; currentParticle < numParticles; currentParticle += 2)
		force(currentParticle, cloud->getx2_pd(currentParticle), cloud->gety2_pd(currentParticle));
}

void RectConfinementForce::force3(const double currentTime)
{
	for (unsigned int currentParticle = 0, numParticles = cloud->n; currentParticle < numParticles; currentParticle += 2)
		force(currentParticle, cloud->getx3_pd(currentParticle), cloud->gety3_pd(currentParticle));
}

void RectConfinementForce::force4(const double currentTime)
{
	for (unsigned int currentParticle = 0, numParticles = cloud->n; currentParticle < numParticles; currentParticle += 2)
		force(currentParticle, cloud->getx4_pd(currentParticle), cloud->gety4_pd(currentParticle));
}

inline void RectConfinementForce::force(const unsigned int currentParticle, const __m128d currentPositionX, const __m128d currentPositionY)
{
	double * const pFx = cloud->forceX + currentParticle;
	double * const pFy = cloud->forceY + currentParticle;

	_mm_store_pd(pFx, _mm_load_pd(pFx) + _mm_set1_pd(confineX)*currentPositionX);
	_mm_store_pd(pFy, _mm_load_pd(pFy) + _mm_set1_pd(confineY)*currentPositionY);
}

void RectConfinementForce::writeForce(fitsfile * const file, int * const error) const
{
	//move to primary HDU:
	if(!*error)
		//file, # indicating primary HDU, HDU type, error
 		fits_movabs_hdu(file, 1, IMAGE_HDU, error);

	//add flag indicating that the rectangular confinement force is used:
	if(!*error)
	{
		long forceFlags = 0;
		fits_read_key_lng(file, const_cast<char *> ("FORCES"), &forceFlags, NULL, error);

		//add RectConfinementForce bit:
		forceFlags |= RectConfinementForceFlag;		//compound bitwise OR

		if(*error == KEY_NO_EXIST || *error == VALUE_UNDEFINED)
			*error = 0;			//clear above error.

		//add or update keyword:
		if(!*error)
			fits_update_key(file, TLONG, const_cast<char *> ("FORCES"), &forceFlags, const_cast<char *> ("Force configuration."), error);
	}

	if(!*error)
	{
		//file, key name, value, precision (scientific format), comment
		fits_write_key_dbl(file, const_cast<char *> ("confineConstX"), confineX, 6, const_cast<char *> ("[N/m] (RectConfinementForce)"), error);
		fits_write_key_dbl(file, const_cast<char *> ("confineConstY"), confineY, 6, const_cast<char *> ("[N/m] (RectConfinementForce)"), error);
	}
}

void RectConfinementForce::readForce(fitsfile * const file, int * const error)
{
	//move to primary HDU:
	if(!*error)
		//file, # indicating primary HDU, HDU type, error
 		fits_movabs_hdu(file, 1, IMAGE_HDU, error);

	if(!*error)
	{
		//file, key name, value, don't read comment, error
		fits_read_key_dbl(file, const_cast<char *> ("confineConstX"), &confineX, NULL, error);
		fits_read_key_dbl(file, const_cast<char *> ("confineConstY"), &confineY, NULL, error);
	}
}
<|endoftext|>


<commit_before>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkMemoryLimitImageDataStreamer.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
#include "vtkMemoryLimitImageDataStreamer.h"

#include "vtkAlgorithmOutput.h"
#include "vtkCommand.h"
#include "vtkExtentTranslator.h"
#include "vtkImageData.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkObjectFactory.h"
#include "vtkPipelineSize.h"
#include "vtkStreamingDemandDrivenPipeline.h"

vtkCxxRevisionMacro(vtkMemoryLimitImageDataStreamer, "1.8");
vtkStandardNewMacro(vtkMemoryLimitImageDataStreamer);

//----------------------------------------------------------------------------
vtkMemoryLimitImageDataStreamer::vtkMemoryLimitImageDataStreamer()
{
  // Set a default memory limit of 50 Megabytes
  this->MemoryLimit = 50000;
}

//----------------------------------------------------------------------------
void vtkMemoryLimitImageDataStreamer::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os,indent);

  os << indent << "MemoryLimit (in kb): " << this->MemoryLimit << endl;
}

//----------------------------------------------------------------------------
#ifndef VTK_USE_EXECUTIVES
void vtkMemoryLimitImageDataStreamer::UpdateData(vtkDataObject *out)
{
  // find the right number of pieces
  if (!this->GetInput())
    {
    return;
    }

  vtkImageData *input = this->GetInput();
  vtkExtentTranslator *translator = this->GetExtentTranslator();
  translator->SetWholeExtent(out->GetUpdateExtent());

  vtkPipelineSize *sizer = vtkPipelineSize::New();
  this->NumberOfStreamDivisions = 1;
  unsigned long oldSize, size = 0;
  float ratio;
  translator->SetPiece(0);

  // watch for the limiting case where the size is the maximum size
  // represented by an unsigned long. In that case we do not want to do the
  // ratio test. We actual test for size < 0.5 of the max unsigned long which
  // would indicate that oldSize is about at max unsigned long.
  unsigned long maxSize;
  maxSize = (((unsigned long)0x1) << (8*sizeof(unsigned long) - 1));

  // we also have to watch how many pieces we are creating. Since
  // NumberOfStreamDivisions is an int, it cannot be more that say 2^31
  // (which is a bit much anyhow) so we also stop if the number of pieces is
  // too large.
  int count = 0;

  // double the number of pieces until the size fits in memory
  // or the reduction in size falls to 20%
  do
    {
    oldSize = size;
    translator->SetNumberOfPieces(this->NumberOfStreamDivisions);
    translator->PieceToExtentByPoints();
    input->SetUpdateExtent(translator->GetExtent());
    input->PropagateUpdateExtent();
    size = sizer->GetEstimatedSize(this->GetInput());
    // watch for the first time through
    if (!oldSize)
      {
      ratio = 0.5;
      }
    // otherwise the normal ratio calculation
    else
      {
      ratio = size/(float)oldSize;
      }
    this->NumberOfStreamDivisions = this->NumberOfStreamDivisions*2;
    count++;
    }
  while (size > this->MemoryLimit &&
         (size < maxSize && ratio < 0.8) && count < 29);

  // undo the last *2
  this->NumberOfStreamDivisions = this->NumberOfStreamDivisions/2;

  // now call the superclass
  this->vtkImageDataStreamer::UpdateData(out);

  sizer->Delete();
}
#endif

int vtkMemoryLimitImageDataStreamer::ProcessUpstreamRequest(
  vtkInformation *request,
  vtkInformationVector *inputVector,
  vtkInformationVector *outputVector)
{
#ifdef VTK_USE_EXECUTIVES
  if(request->Has(vtkStreamingDemandDrivenPipeline::REQUEST_UPDATE_EXTENT()))
    {
    if (this->CurrentDivision == 0)
      {
      // we must set the extent on the input
      vtkInformation* outInfo = outputVector->GetInformationObject(0);

      // get the requested update extent
      int outExt[6];
      outInfo->Get(vtkStreamingDemandDrivenPipeline::UPDATE_EXTENT(), outExt);

      vtkInformation* inInfo = inputVector->GetInformationObject(0)
        ->Get(vtkAlgorithm::INPUT_CONNECTION_INFORMATION())
        ->GetInformationObject(0);
      vtkImageData *input =
        vtkImageData::SafeDownCast(inInfo->Get(vtkDataObject::DATA_OBJECT()));

      vtkExtentTranslator *translator = this->GetExtentTranslator();
      translator->SetWholeExtent(outExt);

      vtkPipelineSize *sizer = vtkPipelineSize::New();
      this->NumberOfStreamDivisions = 1;
      unsigned long oldSize, size = 0;
      float ratio;
      translator->SetPiece(0);

      // watch for the limiting case where the size is the maximum size
      // represented by an unsigned long. In that case we do not want to do
      // the ratio test. We actual test for size < 0.5 of the max unsigned
      // long which would indicate that oldSize is about at max unsigned
      // long.
      unsigned long maxSize;
      maxSize = (((unsigned long)0x1) << (8*sizeof(unsigned long) - 1));

      // we also have to watch how many pieces we are creating. Since
      // NumberOfStreamDivisions is an int, it cannot be more that say 2^31
      // (which is a bit much anyhow) so we also stop if the number of pieces
      // is too large.
      int count = 0;

      // double the number of pieces until the size fits in memory
      // or the reduction in size falls to 20%
      do
        {
        oldSize = size;
        translator->SetNumberOfPieces(this->NumberOfStreamDivisions);
        translator->PieceToExtentByPoints();

        int inExt[6];
        translator->GetExtent(inExt);
        // set the update extent
        inInfo->Set(
          vtkStreamingDemandDrivenPipeline::UPDATE_EXTENT(), inExt, 6);

        // then propagate it
        vtkAlgorithm *alg = input->GetProducerPort()->GetProducer();
        int index = input->GetProducerPort()->GetIndex();
        vtkStreamingDemandDrivenPipeline *exec =
          vtkStreamingDemandDrivenPipeline::SafeDownCast(alg->GetExecutive());
        exec->PropagateUpdateExtent(index);

        size = sizer->GetEstimatedSize(this->GetInput());
        // watch for the first time through
        if (!oldSize)
          {
          ratio = 0.5;
          }
        // otherwise the normal ratio calculation
        else
          {
          ratio = size/(float)oldSize;
          }
        this->NumberOfStreamDivisions = this->NumberOfStreamDivisions*2;
        count++;
        }
      while (size > this->MemoryLimit &&
             (size < maxSize && ratio < 0.8) && count < 29);

      // undo the last *2
      this->NumberOfStreamDivisions = this->NumberOfStreamDivisions/2;
      }
    return
      this->Superclass::ProcessUpstreamRequest(request,
                                               inputVector,
                                               outputVector);
    }
#else
  (void)request;
#endif
  return 0;
}
<commit_msg>ERR: Fixed unused parameter warnings.<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkMemoryLimitImageDataStreamer.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
#include "vtkMemoryLimitImageDataStreamer.h"

#include "vtkAlgorithmOutput.h"
#include "vtkCommand.h"
#include "vtkExtentTranslator.h"
#include "vtkImageData.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkObjectFactory.h"
#include "vtkPipelineSize.h"
#include "vtkStreamingDemandDrivenPipeline.h"

vtkCxxRevisionMacro(vtkMemoryLimitImageDataStreamer, "1.9");
vtkStandardNewMacro(vtkMemoryLimitImageDataStreamer);

//----------------------------------------------------------------------------
vtkMemoryLimitImageDataStreamer::vtkMemoryLimitImageDataStreamer()
{
  // Set a default memory limit of 50 Megabytes
  this->MemoryLimit = 50000;
}

//----------------------------------------------------------------------------
void vtkMemoryLimitImageDataStreamer::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os,indent);

  os << indent << "MemoryLimit (in kb): " << this->MemoryLimit << endl;
}

//----------------------------------------------------------------------------
#ifndef VTK_USE_EXECUTIVES
void vtkMemoryLimitImageDataStreamer::UpdateData(vtkDataObject *out)
{
  // find the right number of pieces
  if (!this->GetInput())
    {
    return;
    }

  vtkImageData *input = this->GetInput();
  vtkExtentTranslator *translator = this->GetExtentTranslator();
  translator->SetWholeExtent(out->GetUpdateExtent());

  vtkPipelineSize *sizer = vtkPipelineSize::New();
  this->NumberOfStreamDivisions = 1;
  unsigned long oldSize, size = 0;
  float ratio;
  translator->SetPiece(0);

  // watch for the limiting case where the size is the maximum size
  // represented by an unsigned long. In that case we do not want to do the
  // ratio test. We actual test for size < 0.5 of the max unsigned long which
  // would indicate that oldSize is about at max unsigned long.
  unsigned long maxSize;
  maxSize = (((unsigned long)0x1) << (8*sizeof(unsigned long) - 1));

  // we also have to watch how many pieces we are creating. Since
  // NumberOfStreamDivisions is an int, it cannot be more that say 2^31
  // (which is a bit much anyhow) so we also stop if the number of pieces is
  // too large.
  int count = 0;

  // double the number of pieces until the size fits in memory
  // or the reduction in size falls to 20%
  do
    {
    oldSize = size;
    translator->SetNumberOfPieces(this->NumberOfStreamDivisions);
    translator->PieceToExtentByPoints();
    input->SetUpdateExtent(translator->GetExtent());
    input->PropagateUpdateExtent();
    size = sizer->GetEstimatedSize(this->GetInput());
    // watch for the first time through
    if (!oldSize)
      {
      ratio = 0.5;
      }
    // otherwise the normal ratio calculation
    else
      {
      ratio = size/(float)oldSize;
      }
    this->NumberOfStreamDivisions = this->NumberOfStreamDivisions*2;
    count++;
    }
  while (size > this->MemoryLimit &&
         (size < maxSize && ratio < 0.8) && count < 29);

  // undo the last *2
  this->NumberOfStreamDivisions = this->NumberOfStreamDivisions/2;

  // now call the superclass
  this->vtkImageDataStreamer::UpdateData(out);

  sizer->Delete();
}
#endif

int vtkMemoryLimitImageDataStreamer::ProcessUpstreamRequest(
  vtkInformation *request,
  vtkInformationVector *inputVector,
  vtkInformationVector *outputVector)
{
#ifdef VTK_USE_EXECUTIVES
  if(request->Has(vtkStreamingDemandDrivenPipeline::REQUEST_UPDATE_EXTENT()))
    {
    if (this->CurrentDivision == 0)
      {
      // we must set the extent on the input
      vtkInformation* outInfo = outputVector->GetInformationObject(0);

      // get the requested update extent
      int outExt[6];
      outInfo->Get(vtkStreamingDemandDrivenPipeline::UPDATE_EXTENT(), outExt);

      vtkInformation* inInfo = inputVector->GetInformationObject(0)
        ->Get(vtkAlgorithm::INPUT_CONNECTION_INFORMATION())
        ->GetInformationObject(0);
      vtkImageData *input =
        vtkImageData::SafeDownCast(inInfo->Get(vtkDataObject::DATA_OBJECT()));

      vtkExtentTranslator *translator = this->GetExtentTranslator();
      translator->SetWholeExtent(outExt);

      vtkPipelineSize *sizer = vtkPipelineSize::New();
      this->NumberOfStreamDivisions = 1;
      unsigned long oldSize, size = 0;
      float ratio;
      translator->SetPiece(0);

      // watch for the limiting case where the size is the maximum size
      // represented by an unsigned long. In that case we do not want to do
      // the ratio test. We actual test for size < 0.5 of the max unsigned
      // long which would indicate that oldSize is about at max unsigned
      // long.
      unsigned long maxSize;
      maxSize = (((unsigned long)0x1) << (8*sizeof(unsigned long) - 1));

      // we also have to watch how many pieces we are creating. Since
      // NumberOfStreamDivisions is an int, it cannot be more that say 2^31
      // (which is a bit much anyhow) so we also stop if the number of pieces
      // is too large.
      int count = 0;

      // double the number of pieces until the size fits in memory
      // or the reduction in size falls to 20%
      do
        {
        oldSize = size;
        translator->SetNumberOfPieces(this->NumberOfStreamDivisions);
        translator->PieceToExtentByPoints();

        int inExt[6];
        translator->GetExtent(inExt);
        // set the update extent
        inInfo->Set(
          vtkStreamingDemandDrivenPipeline::UPDATE_EXTENT(), inExt, 6);

        // then propagate it
        vtkAlgorithm *alg = input->GetProducerPort()->GetProducer();
        int index = input->GetProducerPort()->GetIndex();
        vtkStreamingDemandDrivenPipeline *exec =
          vtkStreamingDemandDrivenPipeline::SafeDownCast(alg->GetExecutive());
        exec->PropagateUpdateExtent(index);

        size = sizer->GetEstimatedSize(this->GetInput());
        // watch for the first time through
        if (!oldSize)
          {
          ratio = 0.5;
          }
        // otherwise the normal ratio calculation
        else
          {
          ratio = size/(float)oldSize;
          }
        this->NumberOfStreamDivisions = this->NumberOfStreamDivisions*2;
        count++;
        }
      while (size > this->MemoryLimit &&
             (size < maxSize && ratio < 0.8) && count < 29);

      // undo the last *2
      this->NumberOfStreamDivisions = this->NumberOfStreamDivisions/2;
      }
    return
      this->Superclass::ProcessUpstreamRequest(request,
                                               inputVector,
                                               outputVector);
    }
  return 0;
#else
  return this->Superclass::ProcessUpstreamRequest(request,
                                                  inputVector,
                                                  outputVector);
#endif
}
<|endoftext|>


<commit_before>/* -------------------------------------------------------------------------- *
 *                               Simbody(tm)                                  *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK biosimulation toolkit originating from           *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
 *                                                                            *
 * Portions copyright (c) 2008-12 Stanford University and the Authors.        *
 * Authors: Peter Eastman                                                     *
 * Contributors:                                                              *
 *                                                                            *
 * 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.                                             *
 * -------------------------------------------------------------------------- */

#include "SimTKsimbody.h"

using namespace SimTK;
using namespace std;

const Real TOL = 1e-5;

#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}

template <class T>
void assertEqual(T val1, T val2) {
    ASSERT(abs(val1-val2) < TOL || abs(val1-val2)/max(abs(val1), abs(val2)) < TOL);
}

template <int N>
void assertEqual(Vec<N> val1, Vec<N> val2) {
    for (int i = 0; i < N; ++i)
        assertEqual(val1[i], val2[i]);
}

void testForces() {
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    GeneralContactSubsystem contacts(system);
    GeneralForceSubsystem forces(system);

    // Create a triangle mesh in the shape of a pyramid, with the
    // square base having area 1 (split into two triangles).

    vector<Vec3> vertices;
    vertices.push_back(Vec3(0, 0, 0));
    vertices.push_back(Vec3(1, 0, 0));
    vertices.push_back(Vec3(1, 0, 1));
    vertices.push_back(Vec3(0, 0, 1));
    vertices.push_back(Vec3(0.5, 1, 0.5));
    vector<int> faceIndices;
    int faces[6][3] = {{0, 1, 2}, {0, 2, 3}, {1, 0, 4},
                       {2, 1, 4}, {3, 2, 4}, {0, 3, 4}};
    for (int i = 0; i < 6; i++)
        for (int j = 0; j < 3; j++)
            faceIndices.push_back(faces[i][j]);

    // Create the mobilized bodies and configure the contact model.

    Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
    ContactSetIndex setIndex = contacts.createContactSet();
    MobilizedBody::Translation mesh(matter.updGround(), Transform(), body, Transform());
    contacts.addBody(setIndex, mesh, ContactGeometry::TriangleMesh(vertices, faceIndices), Transform());
    contacts.addBody(setIndex, matter.updGround(), ContactGeometry::HalfSpace(), Transform(Rotation(-0.5*Pi, ZAxis), Vec3(0))); // y < 0
    ElasticFoundationForce ef(forces, contacts, setIndex);
    Real stiffness = 1e9, dissipation = 0.01, us = 0.1, ud = 0.05, uv = 0.01, vt = 0.01;
    ef.setBodyParameters(ContactSurfaceIndex(0), stiffness, dissipation, us, ud, uv);
    ef.setTransitionVelocity(vt);
    ASSERT(ef.getTransitionVelocity() == vt);
    State state = system.realizeTopology();

    // Position the pyramid at a variety of positions and check the normal
    // force.

    for (Real depth = -0.1; depth < 0.1; depth += 0.01) {
        mesh.setQToFitTranslation(state, Vec3(0, -depth, 0));
        system.realize(state, Stage::Dynamics);
        Real f = 0;
        if (depth > 0)
            f = stiffness*depth;
        assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[mesh.getMobilizedBodyIndex()][1], Vec3(0, f, 0));
        assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[matter.getGround().getMobilizedBodyIndex()][1], Vec3(0, -f, 0));
    }

    // Now do it with a vertical velocity and see if the dissipation force is correct.

    for (Real depth = -0.105; depth < 0.1; depth += 0.01) {
        mesh.setQToFitTranslation(state, Vec3(0, -depth, 0));
        for (Real v = -1.0; v <= 1.0; v += 0.1) {
            mesh.setUToFitLinearVelocity(state, Vec3(0, -v, 0));
            system.realize(state, Stage::Dynamics);
            Real f = (depth > 0 ? stiffness*depth*(1+dissipation*v) : 0);
            if (f < 0)
                f = 0;
            assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[mesh.getMobilizedBodyIndex()][1], Vec3(0, f, 0));
        }
    }

    // Do it with a horizontal velocity and see if the friction force is correct.

    Vector_<SpatialVec> expectedForce(matter.getNumBodies());
    for (Real depth = -0.105; depth < 0.1; depth += 0.01) {
        mesh.setQToFitTranslation(state, Vec3(0, -depth, 0));
        Real fh = 0;
        if (depth > 0)
            fh = stiffness*depth;
        for (Real v = -1.0; v <= 1.0; v += 0.1) {
            mesh.setUToFitLinearVelocity(state, Vec3(v, 0, 0));
            system.realize(state, Stage::Dynamics);
            const Real vrel = std::abs(v/vt);
            Real ff = (v < 0 ? 1 : -1)*fh*(std::min(vrel, 1.0)*(ud+2*(us-ud)/(1+vrel*vrel))+uv*std::fabs(v));
            const Vec3 totalForce = Vec3(ff, fh, 0);
            expectedForce = SpatialVec(Vec3(0), Vec3(0));
            Vec3 contactPoint1 = mesh.findStationAtGroundPoint(state, Vec3(2.0/3.0, 0, 1.0/3.0));
            mesh.applyForceToBodyPoint(state, contactPoint1, 0.5*totalForce, expectedForce);
            Vec3 contactPoint2 = mesh.findStationAtGroundPoint(state, Vec3(1.0/3.0, 0, 2.0/3.0));
            mesh.applyForceToBodyPoint(state, contactPoint2, 0.5*totalForce, expectedForce);
            SpatialVec actualForce = system.getRigidBodyForces(state, Stage::Dynamics)[mesh.getMobilizedBodyIndex()];
            assertEqual(actualForce[0], expectedForce[mesh.getMobilizedBodyIndex()][0]);
            assertEqual(actualForce[1], expectedForce[mesh.getMobilizedBodyIndex()][1]);
        }
    }
}

int main() {
    try {
        testForces();
    }
    catch(const std::exception& e) {
        cout << "exception: " << e.what() << endl;
        return 1;
    }
    cout << "Done" << endl;
    return 0;
}
<commit_msg>[master] Added testEffSphereOnPlaneOldFormulation test<commit_after>/* -------------------------------------------------------------------------- *
 *                               Simbody(tm)                                  *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK biosimulation toolkit originating from           *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
 *                                                                            *
 * Portions copyright (c) 2008-12 Stanford University and the Authors.        *
 * Authors: Peter Eastman                                                     *
 * Contributors:                                                              *
 *                                                                            *
 * 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.                                             *
 * -------------------------------------------------------------------------- */

#include "SimTKsimbody.h"

using namespace SimTK;
using namespace std;

const Real TOL = 1e-5;

#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}

template <class T>
void assertEqual(T val1, T val2) {
    ASSERT(abs(val1-val2) < TOL || abs(val1-val2)/max(abs(val1), abs(val2)) < TOL);
}

template <int N>
void assertEqual(Vec<N> val1, Vec<N> val2) {
    for (int i = 0; i < N; ++i)
        assertEqual(val1[i], val2[i]);
}

void testForces() {
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    GeneralContactSubsystem contacts(system);
    GeneralForceSubsystem forces(system);

    // Create a triangle mesh in the shape of a pyramid, with the
    // square base having area 1 (split into two triangles).

    vector<Vec3> vertices;
    vertices.push_back(Vec3(0, 0, 0));
    vertices.push_back(Vec3(1, 0, 0));
    vertices.push_back(Vec3(1, 0, 1));
    vertices.push_back(Vec3(0, 0, 1));
    vertices.push_back(Vec3(0.5, 1, 0.5));
    vector<int> faceIndices;
    int faces[6][3] = {{0, 1, 2}, {0, 2, 3}, {1, 0, 4},
                       {2, 1, 4}, {3, 2, 4}, {0, 3, 4}};
    for (int i = 0; i < 6; i++)
        for (int j = 0; j < 3; j++)
            faceIndices.push_back(faces[i][j]);

    // Create the mobilized bodies and configure the contact model.

    Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
    ContactSetIndex setIndex = contacts.createContactSet();
    MobilizedBody::Translation mesh(matter.updGround(), Transform(), body, Transform());
    contacts.addBody(setIndex, mesh, ContactGeometry::TriangleMesh(vertices, faceIndices), Transform());
    contacts.addBody(setIndex, matter.updGround(), ContactGeometry::HalfSpace(), Transform(Rotation(-0.5*Pi, ZAxis), Vec3(0))); // y < 0
    ElasticFoundationForce ef(forces, contacts, setIndex);
    Real stiffness = 1e9, dissipation = 0.01, us = 0.1, ud = 0.05, uv = 0.01, vt = 0.01;
    ef.setBodyParameters(ContactSurfaceIndex(0), stiffness, dissipation, us, ud, uv);
    ef.setTransitionVelocity(vt);
    ASSERT(ef.getTransitionVelocity() == vt);
    State state = system.realizeTopology();

    // Position the pyramid at a variety of positions and check the normal
    // force.

    for (Real depth = -0.1; depth < 0.1; depth += 0.01) {
        mesh.setQToFitTranslation(state, Vec3(0, -depth, 0));
        system.realize(state, Stage::Dynamics);
        Real f = 0;
        if (depth > 0)
            f = stiffness*depth;
        assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[mesh.getMobilizedBodyIndex()][1], Vec3(0, f, 0));
        assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[matter.getGround().getMobilizedBodyIndex()][1], Vec3(0, -f, 0));
    }

    // Now do it with a vertical velocity and see if the dissipation force is correct.

    for (Real depth = -0.105; depth < 0.1; depth += 0.01) {
        mesh.setQToFitTranslation(state, Vec3(0, -depth, 0));
        for (Real v = -1.0; v <= 1.0; v += 0.1) {
            mesh.setUToFitLinearVelocity(state, Vec3(0, -v, 0));
            system.realize(state, Stage::Dynamics);
            Real f = (depth > 0 ? stiffness*depth*(1+dissipation*v) : 0);
            if (f < 0)
                f = 0;
            assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[mesh.getMobilizedBodyIndex()][1], Vec3(0, f, 0));
        }
    }

    // Do it with a horizontal velocity and see if the friction force is correct.

    Vector_<SpatialVec> expectedForce(matter.getNumBodies());
    for (Real depth = -0.105; depth < 0.1; depth += 0.01) {
        mesh.setQToFitTranslation(state, Vec3(0, -depth, 0));
        Real fh = 0;
        if (depth > 0)
            fh = stiffness*depth;
        for (Real v = -1.0; v <= 1.0; v += 0.1) {
            mesh.setUToFitLinearVelocity(state, Vec3(v, 0, 0));
            system.realize(state, Stage::Dynamics);
            const Real vrel = std::abs(v/vt);
            Real ff = (v < 0 ? 1 : -1)*fh*(std::min(vrel, 1.0)*(ud+2*(us-ud)/(1+vrel*vrel))+uv*std::fabs(v));
            const Vec3 totalForce = Vec3(ff, fh, 0);
            expectedForce = SpatialVec(Vec3(0), Vec3(0));
            Vec3 contactPoint1 = mesh.findStationAtGroundPoint(state, Vec3(2.0/3.0, 0, 1.0/3.0));
            mesh.applyForceToBodyPoint(state, contactPoint1, 0.5*totalForce, expectedForce);
            Vec3 contactPoint2 = mesh.findStationAtGroundPoint(state, Vec3(1.0/3.0, 0, 2.0/3.0));
            mesh.applyForceToBodyPoint(state, contactPoint2, 0.5*totalForce, expectedForce);
            SpatialVec actualForce = system.getRigidBodyForces(state, Stage::Dynamics)[mesh.getMobilizedBodyIndex()];
            assertEqual(actualForce[0], expectedForce[mesh.getMobilizedBodyIndex()][0]);
            assertEqual(actualForce[1], expectedForce[mesh.getMobilizedBodyIndex()][1]);
        }
    }
}

/**
 * @brief This test compares the numerical result of a sphere
 *        in contact with a plane, using the elastic foundation
 *        model.
 *        The analytical solution of this problem is given by
 *        the product of the stiffness with the volume of
 *        the sphere in the plane, i.e the volume of a spherical
 *        cap.
 *        The volume of a spherical cap is:
 *          Vcap = Pi*h*h/3.0*(3.0*r-h)
 *        where
 *          r is the radis of the sphere
 *          h the height of the cap. In our case, the penetration
 *          depth
 * @note If we want to go further, we can observe that doubling
 *       the penetration depth results in multiplying the normal
 *       effort by 4.
 *       This is different from Hertz theory, where doubling the
 *       penetration depth results in multiplying
 *       the normal effort by 2^(3/2)~2.68
 *
 */
void testEffSphereOnPlaneOldFormulation(bool verbose = false);
void testEffSphereOnPlaneOldFormulation(bool verbose)
{
    // Material properties for sphere
    const Real stiffness = 1e9;
    const Real dissipation = 0.0, us = 0.0, ud = 0.0, uv = 0.0, vt = 0.0;
    // Sphere radius
    const Real radius = 1.0;
    // Define initial penetration
    const Real initialPenetration = 0.002;
    const int maxLevel = 6;
    // Define some tolerances for each level in %
    const Real tolerances[6]= {0.15, 0.07, 0.03, 0.02, 0.01, 0.02};
    for (int i=0;i<maxLevel;++i)
    {
        // For each level, penetration is double
        const Real penetration = initialPenetration * pow(2.0,(Real)i);
        // Creation of the classical problem
        MultibodySystem system;
        SimbodyMatterSubsystem matter(system);
        GeneralContactSubsystem contacts(system);
        GeneralForceSubsystem forces(system);
        const ContactSetIndex setIndex = contacts.createContactSet();
        // Creation a sphere with 6 level of refinement
        const PolygonalMesh sphereMesh(PolygonalMesh::createSphereMesh(radius, 6));
        // Create the mobilized bodies and configure the contact model.
        const Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
        const MobilizedBody::Translation mesh(matter.updGround(), Transform(), body, Transform());
        contacts.addBody(setIndex, mesh, ContactGeometry::TriangleMesh(sphereMesh), Transform());
        contacts.addBody(setIndex, matter.updGround(), ContactGeometry::HalfSpace(),
                         Transform(Rotation(-0.5*Pi, ZAxis), Vec3(0.0,penetration-radius,0.0))); // y < penetration-radius
        ElasticFoundationForce ef(forces, contacts, setIndex);
        ef.setBodyParameters(ContactSurfaceIndex(0), stiffness, dissipation, us, ud, uv);
        ef.setTransitionVelocity(vt);
        const State state = system.realizeTopology();
        system.realize(state, Stage::Dynamics);
        const SpatialVec r = system.getRigidBodyForces(state, Stage::Dynamics)[mesh.getMobilizedBodyIndex()];
        const Real volumeSphericalCap = Pi*penetration*penetration/3.0*(3.0*radius-penetration);
        const Real theoreticalResult = stiffness*volumeSphericalCap;
        const Real numericalResult = r[1][1];
        const Real relativeDifference = abs((numericalResult/theoreticalResult)-1.0);
        if (verbose) {
            cout<<"Effort for penetration : "
                <<penetration*1000.0<<" mm -> F = "<<numericalResult<<" N "
                <<"(theoretical result : "<<theoreticalResult<< " N "
                <<" relative difference : "<<100.0*relativeDifference<<" %)"<<endl;
        }
        ASSERT(abs((numericalResult/theoreticalResult)-1.0)<tolerances[i]);
    }
}

int main() {
    try {
        testForces();
        testEffSphereOnPlaneOldFormulation();
    }
    catch(const std::exception& e) {
        cout << "exception: " << e.what() << endl;
        return 1;
    }
    cout << "Done" << endl;
    return 0;
}
<|endoftext|>


<commit_before>#include "CONTROL_S.H"

#include "LARA.H"
#include "CAMERA.H"
#include "COLLIDE_S.H"
#include "DRAW.H"
#include "SPECIFIC.H"
#include "EFFECTS.H"
#include "CONTROL.H"
#include "LARAMISC.H"

void DrawBinoculars()
{
	S_Warn("[DrawBinoculars] - Unimplemented!\n");
}

void TranslateItem(struct ITEM_INFO* item, short x, short y, short z)
{
	short sin;//$v1
	short cos;//$t0

	cos = COS(item->pos.y_rot);
	sin = SIN(item->pos.y_rot);

	item->pos.x_pos += ((cos * x) + (sin * z)) >> W2V_SHIFT;
	item->pos.y_pos += y;
	item->pos.z_pos += ((-sin * x) + (cos * z)) >> W2V_SHIFT;
}

int GetChange(struct ITEM_INFO* item, struct ANIM_STRUCT* anim)//7D48C
{
	struct CHANGE_STRUCT* change = NULL;//$t1
	struct RANGE_STRUCT* range = NULL;//$v1
	int i = 0;//$t2
	int j = 0;//$t0

	if (item->current_anim_state != item->goal_anim_state)
	{
		change = &changes[anim->change_index];

		if (anim->number_changes > 0)
		{
			//loc_7D4C4
			do
			{
				if (change->goal_anim_state == item->goal_anim_state)
				{
					range = &ranges[change->range_index];
					if (change->number_ranges > 0)
					{
						//loc_7D4E4
						do
						{
							if (item->frame_number < range->start_frame && range->end_frame < item->frame_number)
							{
								//loc_7D4FC
								j++;
								range++;

							}
							else
							{
								//loc_7D524
								item->anim_number = range->link_anim_num;
								item->frame_number = range->link_frame_num;
								return 1;
							}
						} while (j < change->number_ranges);
					}
					//loc_7D50C
				}//loc_7D50C
				change++;
			} while (++i < anim->number_changes);
		}//locret_7D51C
	}//locret_7D51C

	return 0;
}

void AnimateLara(struct ITEM_INFO* item /* s1 */)//7D53C(<)
{
	struct ANIM_STRUCT* anim = &anims[item->anim_number];//$s2
	unsigned short* command;//$s0
	unsigned short data;//$v0
	int numAnimCommands;
	int i;
	int velocity;//$s0
	int speed;//$v0;
	short sin;//$v0
	short cos;//$at

	item->frame_number += 1;

	if (anim->number_changes > 0 && GetChange(item, anim))
	{
		anim = &anims[item->anim_number];
		item->current_anim_state = anim->current_anim_state;
	}
	//loc_7D5BC
	if (anim->frame_end < item->frame_number)
	{
		numAnimCommands = anim->number_commands;
		if (anim->number_commands > 0)
		{
			command = (unsigned short*)&commands[anim->command_index];

			//loc_7D5EC
			do
			{
				data = *command++;

				if (data - 1 == 0)
				{
					//loc_7D628
					TranslateItem(item, command[0], command[1], command[2]);
					command += 3;
					UpdateLaraRoom(item, -381);
				}
				else if (data - 2 == 0)
				{
					//loc_7D650
					item->gravity_status = 1;
					item->fallspeed = command[0];
					item->speed = command[1];
					command += 2;

					if (lara.calc_fallspeed != 0)
					{
						item->fallspeed = lara.calc_fallspeed;
						lara.calc_fallspeed = 0;
					}
				}
				else if (data - 3 == 0)
				{
					//loc_7D684
					if (lara.gun_status != 5)
					{
						lara.gun_status = 0;
					}
				}
				else if (data - 5 == 0 || data - 6 == 0)
				{
					//loc_7D620
					command += 2;
				}

				//loc_7D698
			} while (--numAnimCommands > 0);
		}
		//loc_7D6A4
		item->anim_number = anim->jump_anim_num;
		item->frame_number = anim->jump_frame_num;

		anim = &anims[anim->jump_anim_num];
		item->current_anim_state = anim->current_anim_state;
	}

	//loc_7D6D4
	i = anim->number_commands;
	if (anim->number_commands > 0)
	{
		command = (unsigned short*)&commands[anim->command_index];

		//loc_7D6EC
		do
		{
			data = *command++;

			if (data - 2 == 0)
			{
				//loc_7D7B8
				command += 2;
			}
			else if (data - 5 == 0)
			{
				//loc_7D720
				if (item->frame_number == command[0])
				{
					if ((command[1] & 0xC000))
					{
						if ((command[1] & 0xC000) != 0x4000 || lara.water_surface_dist < 0 &&
							lara.water_surface_dist != -32512)
						{
							//loc_7D754
							if ((command[1] & 0xC000) != 0x8000 || lara.water_surface_dist >= 0 || lara.water_surface_dist == -32512)
							{
								//loc_7D7B8
								command += 2;
							}
						}

					}//loc_7D76C
				}
				else
				{
					//loc_7D7B8
					command += 2;
				}
				//loc_7D76C
				SoundEffect((command[1] & 0x3FFF), &item->pos, 2);
			}
			else if (data - 6 == 0)
			{
				//loc_7D780
				if (item->frame_number == command[0])
				{
					FXType = (command[1] & 0xC000);
					effect_routines[command[1] & 0x3FFF](item);
				}
				else
				{
					//loc_7D7B8
					command += 2;
				}
			}
			else if (data - 1 == 0)
			{
				command += 3;
			}

			//loc_7D7BC
		} while (--i > 0);
	}

	//loc_7D7C8
	velocity = anim->Xvelocity;

	if (anim->Xacceleration != 0)
	{
		velocity += anim->Xacceleration * (item->frame_number - anim->frame_base);
	}
	//loc_7D7EC
	velocity >>= 16;
	if (!item->gravity_status)
	{
		speed = anim->velocity >> 16;
		if (anim->acceleration != 0)
		{
			speed = (anim->velocity + (anim->acceleration * (item->frame_number - anim->frame_base))) >> 16;
		}

		item->speed = speed;
	}
	else
	{
		//loc_7D828
		item->speed = item->speed - ((anim->velocity + (anim->acceleration * (item->frame_number - 1))) >> 16) + ((anim->velocity + (anim->acceleration * (item->frame_number - 1)) + anim->acceleration) >> 16);

		if (item->fallspeed < 128)
		{
			item->fallspeed += 6;
		}
		else
		{
			item->fallspeed += 1;
		}

		//loc_7D870
		item->pos.y_pos += item->fallspeed;
	}
	//loc_7D884
	if (lara.RopePtr != -1)
	{
		DelAlignLaraToRope(item);
	}

	//loc_7D89C
	if (!lara.IsMoving)
	{
		item->pos.x_pos = (item->pos.x_pos + ((SIN(lara.move_angle) * item->speed) >> W2V_SHIFT)) + (((SIN(lara.move_angle) * item->speed) >> W2V_SHIFT) >> W2V_SHIFT);
		item->pos.z_pos = (item->pos.z_pos + ((COS(lara.move_angle) * item->speed) >> W2V_SHIFT)) + ((COS(lara.move_angle + 16384) * velocity) >> W2V_SHIFT);
	}
	//loc_7D934
}<commit_msg>[Specific-PSXPC_N] Fix bad condition in GetChange.<commit_after>#include "CONTROL_S.H"

#include "LARA.H"
#include "CAMERA.H"
#include "COLLIDE_S.H"
#include "DRAW.H"
#include "SPECIFIC.H"
#include "EFFECTS.H"
#include "CONTROL.H"
#include "LARAMISC.H"

void DrawBinoculars()
{
	S_Warn("[DrawBinoculars] - Unimplemented!\n");
}

void TranslateItem(struct ITEM_INFO* item, short x, short y, short z)
{
	short sin;//$v1
	short cos;//$t0

	cos = COS(item->pos.y_rot);
	sin = SIN(item->pos.y_rot);

	item->pos.x_pos += ((cos * x) + (sin * z)) >> W2V_SHIFT;
	item->pos.y_pos += y;
	item->pos.z_pos += ((-sin * x) + (cos * z)) >> W2V_SHIFT;
}

int GetChange(struct ITEM_INFO* item, struct ANIM_STRUCT* anim)//7D48C
{
	struct CHANGE_STRUCT* change = NULL;//$t1
	struct RANGE_STRUCT* range = NULL;//$v1
	int i = 0;//$t2
	int j = 0;//$t0

	if (item->current_anim_state != item->goal_anim_state)
	{
		change = &changes[anim->change_index];

		if (anim->number_changes > 0)
		{
			//loc_7D4C4
			do
			{
				if (change->goal_anim_state == item->goal_anim_state)
				{
					range = &ranges[change->range_index];
					if (change->number_ranges > 0)
					{
						//loc_7D4E4
						do
						{
							if (item->frame_number < range->start_frame || range->end_frame < item->frame_number)
							{
								//loc_7D4FC
								j++;
								range++;

							}
							else
							{
								//loc_7D524
								item->anim_number = range->link_anim_num;
								item->frame_number = range->link_frame_num;
								return 1;
							}
						} while (j < change->number_ranges);
					}
					//loc_7D50C
				}//loc_7D50C
				change++;
			} while (++i < anim->number_changes);
		}//locret_7D51C
	}//locret_7D51C

	return 0;
}

void AnimateLara(struct ITEM_INFO* item /* s1 */)//7D53C(<)
{
	struct ANIM_STRUCT* anim = &anims[item->anim_number];//$s2
	unsigned short* command;//$s0
	unsigned short data;//$v0
	int numAnimCommands;
	int i;
	int velocity;//$s0
	int speed;//$v0;
	short sin;//$v0
	short cos;//$at

	item->frame_number += 1;

	if (anim->number_changes > 0 && GetChange(item, anim))
	{
		anim = &anims[item->anim_number];
		item->current_anim_state = anim->current_anim_state;
	}
	//loc_7D5BC
	if (anim->frame_end < item->frame_number)
	{
		numAnimCommands = anim->number_commands;
		if (anim->number_commands > 0)
		{
			command = (unsigned short*)&commands[anim->command_index];

			//loc_7D5EC
			do
			{
				data = *command++;

				if (data - 1 == 0)
				{
					//loc_7D628
					TranslateItem(item, command[0], command[1], command[2]);
					command += 3;
					UpdateLaraRoom(item, -381);
				}
				else if (data - 2 == 0)
				{
					//loc_7D650
					item->gravity_status = 1;
					item->fallspeed = command[0];
					item->speed = command[1];
					command += 2;

					if (lara.calc_fallspeed != 0)
					{
						item->fallspeed = lara.calc_fallspeed;
						lara.calc_fallspeed = 0;
					}
				}
				else if (data - 3 == 0)
				{
					//loc_7D684
					if (lara.gun_status != 5)
					{
						lara.gun_status = 0;
					}
				}
				else if (data - 5 == 0 || data - 6 == 0)
				{
					//loc_7D620
					command += 2;
				}

				//loc_7D698
			} while (--numAnimCommands > 0);
		}
		//loc_7D6A4
		item->anim_number = anim->jump_anim_num;
		item->frame_number = anim->jump_frame_num;

		anim = &anims[anim->jump_anim_num];
		item->current_anim_state = anim->current_anim_state;
	}

	//loc_7D6D4
	i = anim->number_commands;
	if (anim->number_commands > 0)
	{
		command = (unsigned short*)&commands[anim->command_index];

		//loc_7D6EC
		do
		{
			data = *command++;

			if (data - 2 == 0)
			{
				//loc_7D7B8
				command += 2;
			}
			else if (data - 5 == 0)
			{
				//loc_7D720
				if (item->frame_number == command[0])
				{
					if ((command[1] & 0xC000))
					{
						if ((command[1] & 0xC000) != 0x4000 || lara.water_surface_dist < 0 &&
							lara.water_surface_dist != -32512)
						{
							//loc_7D754
							if ((command[1] & 0xC000) != 0x8000 || lara.water_surface_dist >= 0 || lara.water_surface_dist == -32512)
							{
								//loc_7D7B8
								command += 2;
							}
						}

					}//loc_7D76C
				}
				else
				{
					//loc_7D7B8
					command += 2;
				}
				//loc_7D76C
				SoundEffect((command[1] & 0x3FFF), &item->pos, 2);
			}
			else if (data - 6 == 0)
			{
				//loc_7D780
				if (item->frame_number == command[0])
				{
					FXType = (command[1] & 0xC000);
					effect_routines[command[1] & 0x3FFF](item);
				}
				else
				{
					//loc_7D7B8
					command += 2;
				}
			}
			else if (data - 1 == 0)
			{
				command += 3;
			}

			//loc_7D7BC
		} while (--i > 0);
	}

	//loc_7D7C8
	velocity = anim->Xvelocity;

	if (anim->Xacceleration != 0)
	{
		velocity += anim->Xacceleration * (item->frame_number - anim->frame_base);
	}
	//loc_7D7EC
	velocity >>= 16;
	if (!item->gravity_status)
	{
		speed = anim->velocity >> 16;
		if (anim->acceleration != 0)
		{
			speed = (anim->velocity + (anim->acceleration * (item->frame_number - anim->frame_base))) >> 16;
		}

		item->speed = speed;
	}
	else
	{
		//loc_7D828
		item->speed = item->speed - ((anim->velocity + (anim->acceleration * (item->frame_number - 1))) >> 16) + ((anim->velocity + (anim->acceleration * (item->frame_number - 1)) + anim->acceleration) >> 16);

		if (item->fallspeed < 128)
		{
			item->fallspeed += 6;
		}
		else
		{
			item->fallspeed += 1;
		}

		//loc_7D870
		item->pos.y_pos += item->fallspeed;
	}
	//loc_7D884
	if (lara.RopePtr != -1)
	{
		DelAlignLaraToRope(item);
	}

	//loc_7D89C
	if (!lara.IsMoving)
	{
		item->pos.x_pos = (item->pos.x_pos + ((SIN(lara.move_angle) * item->speed) >> W2V_SHIFT)) + (((SIN(lara.move_angle) * item->speed) >> W2V_SHIFT) >> W2V_SHIFT);
		item->pos.z_pos = (item->pos.z_pos + ((COS(lara.move_angle) * item->speed) >> W2V_SHIFT)) + ((COS(lara.move_angle + 16384) * velocity) >> W2V_SHIFT);
	}
	//loc_7D934
}<|endoftext|>