VectorFloat Derivative::computeDerivative(const VectorFloat &x) {
if( !initialized ) {
errorLog << "computeDerivative(const VectorFloat &x) - Not Initialized!" << std::endl;
return VectorFloat();
}
if( x.size() != numInputDimensions ) {
errorLog << "computeDerivative(const VectorFloat &x) - The Number Of Input Dimensions (" << numInputDimensions << ") does not match the size of the input vector (" << x.size() << ")!" << std::endl;
return VectorFloat();
}
VectorFloat y;
if( filterData ) {
y = filter.filter( x );
} else y = x;
for(UINT n=0; n<numInputDimensions; n++) {
processedData[n] = (y[n]-yy[n])/delta;
yy[n] = y[n];
}
if( derivativeOrder == SECOND_DERIVATIVE ) {
Float tmp = 0;
for(UINT n=0; n<numInputDimensions; n++) {
tmp = processedData[n];
processedData[n] = (processedData[n]-yyy[n])/delta;
yyy[n] = tmp;
}
}
return processedData;
}
VectorFloat DoubleMovingAverageFilter::filter(const VectorFloat &x){
//If the filter has not been initialised then return 0, otherwise filter x and return y
if( !initialized ){
errorLog << "filter(const VectorFloat &x) - The filter has not been initialized!" << std::endl;
return VectorFloat();
}
if( x.getSize() != numInputDimensions ){
errorLog << "filter(const VectorFloat &x) - The size of the input vector (" << x.getSize() << ") does not match that of the number of dimensions of the filter (" << numInputDimensions << ")!" << std::endl;
return VectorFloat();
}
//Perform the first filter
VectorFloat y = filter1.filter( x );
if( y.size() == 0 ) return y;
//Perform the second filter
VectorFloat yy = filter2.filter( y );
if( yy.size() == 0 ) return y;
//Account for the filter lag
const UINT N = y.getSize();
for(UINT i=0; i<N; i++){
yy[i] = y[i] + (y[i] - yy[i]);
processedData[i] = yy[i];
}
return yy;
}
VectorFloat AttitudeLoop::ComputePP(Quaternion qM, VectorFloat omegaM) {
Quaternion qErr;
VectorFloat axisErr;
Serial.print("Printing qRef ");
fQRef.print();
Serial.print("Printing qM ");
qM.print();
qErr = fQRef.conjugate() * qM;
Serial.print("Printing qErr ");
qErr.print();
if(qErr.w < 0) axisErr = VectorFloat(qErr);
else axisErr = -VectorFloat(qErr);
Serial.print("Printing axisErr ");
axisErr.print();
Serial.print("Printing omegaM ");
omegaM.print();
Serial.print("Printing torque without I ");
(axisErr*fPQ - omegaM*fPOmega).print();
fTorque = fI * (axisErr*fPQ - omegaM*fPOmega);
Serial.print("Printing fTorque ");
fTorque.print();
return fTorque;
}
void Duck::updateShotBox()
{
shotbox.Left = sprite.TransformToGlobal(VectorFloat(0, 0)).x;
shotbox.Right = sprite.GetPosition().x + sprite.GetSize().x/2;
shotbox.Top = sprite.TransformToGlobal(VectorFloat(0, 0)).y;
shotbox.Bottom = sprite.GetPosition().y + sprite.GetSize().y/2;
}
VectorFloat TimeseriesBuffer::update(const VectorFloat &x){
if( !initialized ){
errorLog << "update(const VectorFloat &x) - Not Initialized!" << std::endl;
return VectorFloat();
}
if( x.getSize() != numInputDimensions ){
errorLog << "update(const VectorFloat &x)- The Number Of Input Dimensions (" << numInputDimensions << ") does not match the size of the input vector (" << x.getSize() << ")!" << std::endl;
return VectorFloat();
}
//Add the new data to the buffer
dataBuffer.push_back( x );
//Search the buffer for the zero crossing features
UINT colIndex = 0;
for(UINT j=0; j<numInputDimensions; j++){
for(UINT i=0; i<dataBuffer.getSize(); i++){
featureVector[ colIndex++ ] = dataBuffer[i][j];
}
}
//Flag that the feature data has been computed
if( dataBuffer.getBufferFilled() ){
featureDataReady = true;
}else featureDataReady = false;
return featureVector;
}
VectorFloat MovingAverageFilter::filter(const VectorFloat &x){
//If the filter has not been initialised then return 0, otherwise filter x and return y
if( !initialized ){
errorLog << "filter(const VectorFloat &x) - The filter has not been initialized!" << std::endl;
return VectorFloat();
}
if( x.size() != numInputDimensions ){
errorLog << "filter(const VectorFloat &x) - The size of the input vector (" << x.getSize() << ") does not match that of the number of dimensions of the filter (" << numInputDimensions << ")!" << std::endl;
return VectorFloat();
}
if( ++inputSampleCounter > filterSize ) inputSampleCounter = filterSize;
//Add the new value to the buffer
dataBuffer.push_back( x );
for(unsigned int j=0; j<numInputDimensions; j++){
processedData[j] = 0;
for(unsigned int i=0; i<inputSampleCounter; i++) {
processedData[j] += dataBuffer[i][j];
}
processedData[j] /= Float(inputSampleCounter);
}
return processedData;
}
VectorFloat ZeroCrossingCounter::update(const VectorFloat &x){
if( !initialized ){
errorLog << "update(const VectorFloat &x) - Not Initialized!" << std::endl;
return VectorFloat();
}
if( x.getSize() != numInputDimensions ){
errorLog << "update(const VectorFloat &x)- The Number Of Input Dimensions (" << numInputDimensions << ") does not match the size of the input vector (" << x.getSize() << ")!" << std::endl;
return VectorFloat();
}
//Clear the feature vector
std::fill(featureVector.begin(),featureVector.end(),0);
//Update the derivative data and
derivative.computeDerivative( x );
//Dead zone the derivative data
deadZone.filter( derivative.getProcessedData() );
//Add the deadzone data to the buffer
dataBuffer.push_back( deadZone.getProcessedData() );
//Search the buffer for the zero crossing features
for(UINT j=0; j<numInputDimensions; j++){
UINT colIndex = (featureMode == INDEPENDANT_FEATURE_MODE ? (TOTAL_NUM_ZERO_CROSSING_FEATURES*j) : 0);
for(UINT i=1; i<dataBuffer.getSize(); i++){
//Search for a zero crossing
if( (dataBuffer[i][j] > 0 && dataBuffer[i-1][j] <= 0) || (dataBuffer[i][j] < 0 && dataBuffer[i-1][j] >= 0) ){
//Update the zero crossing count
featureVector[ NUM_ZERO_CROSSINGS_COUNTED + colIndex ]++;
//Update the magnitude, search the last 5 values around the zero crossing to make sure we get the maxima of the peak
Float maxValue = 0;
UINT searchSize = i > 5 ? 5 : i;
for(UINT n=0; n<searchSize; n++){
Float value = fabs( dataBuffer[ i-n ][j] );
if( value > maxValue ) maxValue = value;
}
featureVector[ ZERO_CROSSING_MAGNITUDE + colIndex ] += maxValue;
}
}
}
//Flag that the feature data has been computed
featureDataReady = true;
return featureVector;
}
VectorFloat MatrixFloat::multiple(const VectorFloat &b) const{
const unsigned int M = rows;
const unsigned int N = cols;
const unsigned int K = (unsigned int)b.size();
if( N != K ){
warningLog << "multiple(vector b) - The size of b (" << b.size() << ") does not match the number of columns in this matrix (" << N << ")" << std::endl;
return VectorFloat();
}
VectorFloat c(M);
const Float *pb = &b[0];
Float *pc = &c[0];
unsigned int i,j = 0;
for(i=0; i<rows; i++){
pc[i] = 0;
for(j=0; j<cols; j++){
pc[i] += dataPtr[i*cols+j]*pb[j];
}
}
return c;
}
bool TimeseriesBuffer::init(const UINT bufferSize,const UINT numDimensions){
initialized = false;
featureDataReady = false;
if( bufferSize == 0 ){
errorLog << "init(UINT bufferSize,UINT numDimensions) - The bufferSize must be greater than zero!" << std::endl;
return false;
}
if( numDimensions == 0 ){
errorLog << "init(UINT bufferSize,UINT numDimensions) - The numDimensions must be greater than zero!" << std::endl;
return false;
}
//Setup the databuffer
numInputDimensions = numDimensions;
numOutputDimensions = bufferSize * numInputDimensions;
this->bufferSize = bufferSize;
dataBuffer.resize( bufferSize, VectorFloat(numInputDimensions,0) );
featureVector.resize(numOutputDimensions,0);
//Flag that the timeseries buffer has been initialized
initialized = true;
return true;
}
VectorFloat VectorFloat::cross(VectorFloat v){
v = VectorFloat();
v[0] = y*v.z-z*v.y;
v[1] = z*v.x-x*v.z;
v[2] = x*v.y-y*v.x;
return v;
}
bool SavitzkyGolayFilter::init(UINT numLeftHandPoints,UINT numRightHandPoints,UINT derivativeOrder,UINT smoothingPolynomialOrder,UINT numDimensions){
initialized = false;
if( numDimensions == 0 ){
errorLog << "init(Float filterFactor,Float gain,UINT numDimensions) - NumDimensions must be greater than 0!" << std::endl;
return false;
}
this->numPoints = numLeftHandPoints+numRightHandPoints+1;
this->numLeftHandPoints = numLeftHandPoints;
this->numRightHandPoints = numRightHandPoints;
this->derivativeOrder = derivativeOrder;
this->smoothingPolynomialOrder = smoothingPolynomialOrder;
coeff.resize(numPoints);
this->numInputDimensions = numDimensions;
this->numOutputDimensions = numDimensions;
yy.clear();
yy.resize(numDimensions,0);
processedData.clear();
processedData.resize(numDimensions,0);
data.resize(numPoints,VectorFloat(numDimensions,0));
if( !calCoeff() ){
errorLog << "init(UINT NL,UINT NR,UINT LD,UINT M,UINT numDimensions) - Failed to compute filter coefficents!" << std::endl;
return false;
}
initialized = true;
return true;
}
bool MovingAverageFilter::init(UINT filterSize,UINT numDimensions){
//Cleanup the old memory
initialized = false;
inputSampleCounter = 0;
if( filterSize == 0 ){
errorLog << "init(UINT filterSize,UINT numDimensions) - Filter size can not be zero!" << std::endl;
return false;
}
if( numDimensions == 0 ){
errorLog << "init(UINT filterSize,UINT numDimensions) - The number of dimensions must be greater than zero!" << std::endl;
return false;
}
//Resize the filter
this->filterSize = filterSize;
this->numInputDimensions = numDimensions;
this->numOutputDimensions = numDimensions;
processedData.clear();
processedData.resize(numDimensions,0);
initialized = dataBuffer.resize( filterSize, VectorFloat(numInputDimensions,0) );
if( !initialized ){
errorLog << "init(UINT filterSize,UINT numDimensions) - Failed to resize dataBuffer!" << std::endl;
}
return initialized;
}
void Duck::flyOut()
{
if (prevstate == DuckState::FLYING_AROUND) {
float angle = Random::Random(-ANGLE_RANGE, ANGLE_RANGE);
velocity = VectorFloat(speed*cos(angle), -fabs(speed*sin(angle)));
}
sprite.Move(velocity);
}
void Duck::die()
{
sprite.FlipX(false);
sprite.SetSubRect(frames[DuckFrame::SHOT]);
velocity = VectorFloat(0, 0); //Stop moving
if (actiontimer.GetElapsedTime() >= 1) setState(DuckState::FALLING);
}
VectorFloat FFT::getFrequencyBins(const unsigned int sampleRate){
if( !initialized ){ return VectorFloat(); }
VectorFloat freqBins( fftWindowSize );
for(unsigned int i=0; i<fftWindowSize; i++){
freqBins[i] = (i*sampleRate) / fftWindowSize;
}
return freqBins;
}
VectorFloat LowPassFilter::filter(const VectorFloat &x){
if( !initialized ){
errorLog << "filter(const VectorFloat &x) - Not Initialized!" << std::endl;
return VectorFloat();
}
if( x.size() != numInputDimensions ){
errorLog << "filter(const VectorFloat &x) - The Number Of Input Dimensions (" << numInputDimensions << ") does not match the size of the input vector (" << x.size() << ")!" << std::endl;
return VectorFloat();
}
//Exponential moving average filter: lastOutput*alpha + (1.0f-alpha)*input;
for(UINT n=0; n<numInputDimensions; n++){
processedData[n] = (yy[n] * filterFactor) + (x[n] * (1.0 - filterFactor)) * gain;
yy[n] = processedData[n];
}
return processedData;
}
RasterVsVectorState::RasterVsVectorState()
{
//Sets up the raster circle (aka the circle with pixels).
initSprite(rastercircle, sprites, RectInt(0, 16, 32, 48), 1, VectorFloat(SCREEN.GetWidth()/4, CENTER.y));
//Sets up the vector circle
vectorcircle = Shape::Circle(Window.GetWidth()*.75, CENTER.y, 16, Color::Blue);
//Sets the title text
rastertitle.SetText("Raster Vs. Vector Graphics");
}
bool SavitzkyGolayFilter::reset(){
if( initialized ){
data.setAllValues(VectorFloat(numInputDimensions,0));
yy.clear();
yy.resize(numInputDimensions,0);
processedData.clear();
processedData.resize(numInputDimensions,0);
return true;
}
return false;
}
bool TimeDomainFeatures::init(UINT bufferLength,UINT numFrames,UINT numDimensions,bool offsetInput,bool useMean,bool useStdDev,bool useEuclideanNorm,bool useRMS){
initialized = false;
if( numFrames > bufferLength ){
errorLog << "init(...) - The number of numFrames parameter can not be larger than the buffer length parameter!" << std::endl;
return false;
}
if( bufferLength % numFrames != 0 ){
errorLog << "init(...) - The buffer length parameter must be divisible with no remainders by the number of numFrames parameter!" << std::endl;
return false;
}
this->bufferLength = bufferLength;
this->numFrames = numFrames;
this->numInputDimensions = numDimensions;
this->offsetInput = offsetInput;
this->useMean = useMean;
this->useStdDev = useStdDev;
this->useEuclideanNorm = useEuclideanNorm;
this->useRMS = useRMS;
featureDataReady = false;
//Set the number of output dimensions
numOutputDimensions = 0;
if( useMean ){
numOutputDimensions += numInputDimensions*numFrames;
}
if( useStdDev ){
numOutputDimensions += numInputDimensions*numFrames;
}
if( useEuclideanNorm ){
numOutputDimensions += numInputDimensions*numFrames;
}
if( useRMS ){
numOutputDimensions += numInputDimensions*numFrames;
}
if( numOutputDimensions == 0 ){
errorLog << "init(...) - The numOutputDimensions is zero!" << std::endl;
return false;
}
//Resize the feature vector
featureVector.resize(numOutputDimensions);
//Resize the raw data buffer
dataBuffer.resize( bufferLength, VectorFloat(numInputDimensions,0) );
//Flag that the time domain features has been initialized
initialized = true;
return true;
}
Float DoubleMovingAverageFilter::filter(const Float x){
//If the filter has not been initialised then return 0, otherwise filter x and return y
if( !initialized ){
errorLog << "filter(const Float x) - The filter has not been initialized!" << std::endl;
return 0;
}
VectorFloat y = filter(VectorFloat(1,x));
if( y.getSize() == 0 ) return 0;
return y[0];
}
Float SavitzkyGolayFilter::filter(const Float x){
//If the filter has not been initialised then return 0, otherwise filter x and return y
if( !initialized ){
errorLog << "filter(Float x) - The filter has not been initialized!" << std::endl;
return 0;
}
VectorFloat y = filter(VectorFloat(1,x));
if( y.size() > 0 ) return y[0];
return 0;
}
Float Derivative::computeDerivative(const Float x) {
if( numInputDimensions != 1 ) {
errorLog << "computeDerivative(const Float x) - The Number Of Input Dimensions is not 1! NumInputDimensions: " << numInputDimensions << std::endl;
return 0;
}
VectorFloat y = computeDerivative( VectorFloat(1,x) );
if( y.size() == 0 ) return 0 ;
return y[0];
}
Matrix< VectorFloat > SelfOrganizingMap::getWeightsMatrix() const {
if( !trained ) return Matrix< VectorFloat >();
Matrix< VectorFloat > weights( numClusters, numClusters, VectorFloat(numInputDimensions) );
for(UINT i=0; i<numClusters; i++){
for(UINT j=0; j<numClusters; j++){
for(UINT n=0; n<numInputDimensions; n++){
weights[i][j][n] = neurons[i][j][n];
}
}
}
return weights;
}
bool ContinuousHiddenMarkovModel::reset(){
//Reset the base class
MLBase::reset();
if( trained ){
for(unsigned int i=0; i<observationSequence.getSize(); i++){
observationSequence.push_back( VectorFloat(numInputDimensions,0) );
}
}
return true;
}
bool FFT::update(const Float x){
if( !initialized ){
errorLog << "update(const Float x) - Not initialized!" << std::endl;
return false;
}
if( numInputDimensions != 1 ){
errorLog << "update(const Float x) - The size of the input (1) does not match that of the FeatureExtraction (" << numInputDimensions << ")!" << std::endl;
return false;
}
return update(VectorFloat(1,x));
}
VectorFloat SavitzkyGolayFilter::filter(const VectorFloat &x){
if( !initialized ){
errorLog << "filter(const VectorFloat &x) - Not Initialized!" << std::endl;
return VectorFloat();
}
if( x.size() != numInputDimensions ){
errorLog << "filter(const VectorFloat &x) - The Number Of Input Dimensions (" << numInputDimensions << ") does not match the size of the input vector (" << x.size() << ")!" << std::endl;
return VectorFloat();
}
//Add the new input data to the data buffer
data.push_back( x );
//Filter the data
for(UINT j=0; j<x.size(); j++){
processedData[j] = 0;
for(UINT i=0; i<numPoints; i++)
processedData[j] += data[i][j] * coeff[i];
}
return processedData;
}
VectorFloat HighPassFilter::filter(const VectorFloat &x){
if( !initialized ){
errorLog << "filter(const VectorFloat &x) - Not Initialized!" << std::endl;
return VectorFloat();
}
if( x.size() != numInputDimensions ){
errorLog << "filter(const VectorFloat &x) - The Number Of Input Dimensions (" << numInputDimensions << ") does not match the size of the input vector (" << x.size() << ")!" << std::endl;
return VectorFloat();
}
for(UINT n=0; n<numInputDimensions; n++){
//Compute the new output
yy[n] = filterFactor * (yy[n] + x[n] - xx[n]) * gain;
//Store the current input
xx[n] = x[n];
//Store the current output in processed data so it can be accessed by the base class
processedData[n] = yy[n];
}
return processedData;
}
int main()
{
Block::loadImage("./gfx/block.png");
Block::initMap();
Event event;
Game.Clear(Color(50, 50, 100));
Grid<Block> france(VectorInt(GRID_DIMS, GRID_DIMS),
Block::getImageDims(),
PointFloat(224, 48));
france.setCellSize(VectorFloat(BLOCK_DIMS, BLOCK_DIMS));
Block::initContainer(&france);
for (int i = 0; i < france.getDimensions().x; ++i) {
for (int j = 0; j < france.getDimensions().y; ++j) {
PointInt temp(i, j);
france.set(Block(Block::getRandomColor(7), temp), temp);
}
}
for (int i = 0; i < france.getDimensions().x; ++i)
for (int j = 0; j < france.getDimensions().y; ++j)
Game.Draw(france.get(PointInt(i, j)).getSprite());
while (Game.IsOpened()) {
while (Game.GetEvent(event)) {
if (event.Type == sf::Event::Closed) Game.Close();
}
Game.Clear(Color(50, 50, 100));
for (int i = 0; i < france.getDimensions().x; ++i) {
for (int j = 0; j < france.getDimensions().y; ++j) {
if (Game.GetInput().IsMouseButtonDown(sf::Mouse::Button::Left))
france.get(PointInt(i, j)).handleInput(Game.GetInput());
Game.Draw(france.get(PointInt(i, j)).getSprite());
}
}
Game.Display();
}
//TODO: Begin the main game loop
}
void Duck::flyIn()
{
if (prevstate != DuckState::FLYING_IN) {
sprite.SetPosition(Random::Random(buffer[LEFT], buffer[RIGHT]), START_HEIGHT);
updateShotBox();
float angle = Random::Random(-ANGLE_RANGE, ANGLE_RANGE);
velocity = VectorFloat(speed*cos(angle), -fabs(speed*sin(angle)));
}
if (actiontimer.GetElapsedTime() < 1 && sprite.GetPosition().y > buffer[UP]) {
sprite.Move(velocity);
detectBoundaries();
} else {
setState(DuckState::FLYING_AROUND);
setRandomDirection();
}
}
void Duck::updateAnimation()
{
sprite.FlipX(velocity.x < 0); //If the duck is flying left, flip its sprite
//If the duck is flying...
if (state == DuckState::FLYING_AROUND || state == DuckState::FLYING_IN || state == DuckState::FLYING_OUT) {
if (state == DuckState::FLYING_AROUND) {
//This expression uses a sine wave to animate the duck
sprite.SetSubRect(frames[DuckFrame(lround(sin(25*animationtimer.GetElapsedTime()-.5)+1))]);
} else if (state == DuckState::FLYING_IN || state == DuckState::FLYING_OUT) {
sprite.SetSubRect(frames[DuckFrame(lround(sin(25*animationtimer.GetElapsedTime()-.5)+1)+3)]);
}
if (sounds[DuckSound::FLAP].getStatus() == sf::Sound::Stopped && velocity != VectorFloat(0, 0) &&
SCREEN.Contains(sprite.GetPosition().x, sprite.GetPosition().y))
sounds[DuckSound::FLAP].Play();
}
}
