Vector.nxc

Go to the documentation of this file.

/* -*- Mode: C; indent-tabs-mode: t; c-basic-offset: 4; tab-width: 4 -*- */
/*
    Vector.nxc
    Copyright (C) 2008 Naba Kumar   <naba@gnome.org>

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/

#ifndef __VECTOR_H_
#define __VECTOR_H_

#include "Debug.nxc"
#include "Matrix.nxc"

struct Vector
{
    long x;         
    long y;         
    long theta;     
};

long VectorGet(Vector &a, int i)
{
    if (i == 0) return a.x;
    if (i == 1) return a.y;
    return a.theta;
}

void VectorSet(Vector &a, int i, long value)
{
    if (i == 0) a.x = value;
    if (i == 1) a.y = value;
    a.theta = value;
}

long VectorGetDistance(Vector &a)
{
    long distance = (a.x * a.x) + (a.y * a.y);
    distance = Sqrt(distance);
    return distance;
}

int VectorGetAngle(Vector &a)
{
    long distance = VectorGetDistance(a);
    long distanceRatio = ((a.x * 100)/distance);
    int angle = Acos(distanceRatio);
    if (angle > 0 && a.y < 0) angle = 360 - angle;
    return angle;
}

long VectorGetDistanceVec(Vector &a, Vector &b)
{
    long distance = (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y);
    distance = Sqrt(distance);
    return distance;
}

long VectorGetAngleVec(Vector &a, Vector &b)
{
    long distance = VectorGetDistanceVec(a, b);
    long distanceRatio = (((b.x - a.x) * 100)/distance);
    int angle = Acos(distanceRatio);
    if (angle > 0 && (b.y - a.y) < 0) angle = 360 - angle;
    return angle;
}

void VectorInit(Vector &a, long x, long y, long theta)
{
    a.x = x;
    a.y = y;
    a.theta = theta;
}

void VectorInitPolar(Vector &a, long distance, int angle, int theta)
{
    a.x = (distance * Cos(angle))/100;
    a.y = (distance * Sin(angle))/100;
    a.theta = theta;
}

void VectorInitBound(Vector &retVec, long x, long y)
{
    retVec.x = x;
    retVec.y = y;
    retVec.theta = VectorGetAngle(retVec);
}

void VectorTranslate(Vector &a, long distance, int angle)
{
    a.x = a.x + ((distance * Cos(angle))/100);
    a.y = a.y + ((distance * Sin(angle))/100);
}

void VectorAdd(Vector &a, Vector &b)
{
    a.x += b.x;
    a.y += b.y;
    a.theta += b.theta;
}

void VectorSubtract(Vector &a, Vector &b)
{
    a.x -= b.x;
    a.y -= b.y;
    a.theta -= b.theta;
}

void VectorScale(Vector &a, long S)
{
    a.x *= S;
    a.y *= S;
}

void VectorReduce(Vector &a, long S)
{
    a.x /= S;
    a.y /= S;
}

void VectorRotate(Vector &vec, long theta)
{
    long newX = (vec.x * Cos(theta) - vec.y * Sin(theta))/100;
    long newY = (vec.x * Sin(theta) + vec.y * Cos(theta))/100;
    vec.x = newX;
    vec.y = newY;
}

void VectorRotateAtPoint(Vector &vec, long theta, long pivotX, long pivotY)
{
    vec.x -= pivotX;
    vec.y -= pivotY;
    long newX = (vec.x * Cos(theta) - vec.y * Sin(theta))/100;
    long newY = (vec.x * Sin(theta) + vec.y * Cos(theta))/100;
    vec.x = newX + pivotX;
    vec.y = newY + pivotY;
}

void VectorTransform(Vector &vec, Matrix &matrix, Vector &retVec)
{
    ASSERT(matrix.rows == 3 && matrix.cols == 3,
           "Transformation matrix is not 3x3");
    
    retVec.x =     MatrixGet(matrix, 0, 0) * vec.x +
                   MatrixGet(matrix, 0, 1) * vec.y +
                   MatrixGet(matrix, 0, 2) * vec.theta;
    retVec.y =     MatrixGet(matrix, 1, 0) * vec.x +
                   MatrixGet(matrix, 1, 1) * vec.y +
                   MatrixGet(matrix, 1, 2) * vec.theta;
    retVec.theta = MatrixGet(matrix, 2, 0) * vec.x +
                   MatrixGet(matrix, 2, 1) * vec.y +
                   MatrixGet(matrix, 2, 2) * vec.theta;
}

void VectorPrint(Vector &vec, string name, int waitPeriod)
{
    TextOut(0, LCD_LINE1, name, true);
    NumOut(0, LCD_LINE2, vec.x, false);
    NumOut(0, LCD_LINE3, vec.y, false);
    NumOut(0, LCD_LINE4, vec.theta, false);
    Wait(waitPeriod);
}

#endif /* __VECTOR_H_ */