I'm looking for a robust function to calculate the difference(delta) between an object and a point.
For example, it there was an object at point A with an orientation of 1.2 Rad, what would be the required angle for the object to turn in order to face Point B.
Furthermore, I'm working in a odd coordinate system where north(0 Rad) faces towards +X, the image below shows this.
I understand the fundamentals, but I'm struggling to produce something robust.
My c++ function template look like this,
float Robot::getDeltaHeading(float _x1, float _y1, float _x2, float _y2, float _currentHeading) {
//TODO:
return xxxxxxx;
}
Any help would be appreciated.
Cheers in Advance.
Here's the answer.
float Robot::getDeltaHeading(float _x1, float _y1, float _x2, float _y2, float _currentHeading) {
_currentHeading -= 90;
double Ux = 0.0, Uy = 0.0, Vx = 0.0, Vy = 0.0, d = 0.0;
d = sqrtf(powf(abs(_x1 - _x2), 2) + powf(abs(_y1 - _x2), 2));
Ux = (_x2 - _x1) / d;
Uy = (_y2 - _y1) / d;
Vx = cos(_currentHeading * (3.14159f / 180.0));
Vy = sin(_currentHeading * (3.14159f / 180.0));
auto ans = 90 + (atan2(((Ux * Vy) - (Uy * Vx)), ((Ux * Vx) + (Uy * Vy))) * (180.0 / 3.14159f));
while (ans > 180) ans -= 360;
while (ans < -180) ans += 360;
return ans;
}
Related
I want to make a quaternion based camera. In the internet I found this :
https://www.gamedev.net/resources/_/technical/math-and-physics/a-simple-quaternion-based-camera-r1997
From which I took the code :
typedef struct { float w, x, y, z; } quaternion;
double length(quaternion quat)
{
return sqrt(quat.x * quat.x + quat.y * quat.y +
quat.z * quat.z + quat.w * quat.w);
}
quaternion normalize(quaternion quat)
{
double L = length(quat);
quat.x /= L;
quat.y /= L;
quat.z /= L;
quat.w /= L;
return quat;
}
quaternion conjugate(quaternion quat)
{
quat.x = -quat.x;
quat.y = -quat.y;
quat.z = -quat.z;
return quat;
}
quaternion mult(quaternion A, quaternion B)
{
quaternion C;
C.x = A.w*B.x + A.x*B.w + A.y*B.z - A.z*B.y;
C.y = A.w*B.y - A.x*B.z + A.y*B.w + A.z*B.x;
C.z = A.w*B.z + A.x*B.y - A.y*B.x + A.z*B.w;
C.w = A.w*B.w - A.x*B.x - A.y*B.y - A.z*B.z;
return C;
}
void RotateCamera(double Angle, double x, double y, double z)
{
quaternion temp, quat_view, result;
temp.x = x * sin(Angle/2);
temp.y = y * sin(Angle/2);
temp.z = z * sin(Angle/2);
temp.w = cos(Angle/2);
quat_view.x = View.x;
quat_view.y = View.y;
quat_view.z = View.z;
quat_view.w = 0;
result = mult(mult(temp, quat_view), conjugate(temp));
View.x = result.x;
View.y = result.y;
View.z = result.z;
}
But Im having problems when trying to implement this line :
gluLookAt(Position.x, Position.y, Position.z,
View.x, View.y, View.z, Up.x, Up.y, Up.z).
because I have no idea of what to use as 'Up', I tried with 0,0,0, but it only showed a black screen. Any help is greatly appreciated !
EDIT :
Somewhere on this site I found something like that that does convert a quaternion to a matrix. How can I use this matrix using glMultMatrixf();
float *quat_to_matrix(quaternion quat) {
float matrix[16];
double qx=quat.x;
double qy=quat.y;
double qz=quat.z;
double qw=quat.w;
const double n = 1.0f/sqrt(qx*qx+qy*qy+qz*qz+qw*qw);
qx *= n;
qy *= n;
qz *= n;
qw *= n;
matrix={1.0f - 2.0f*qy*qy - 2.0f*qz*qz, 2.0f*qx*qy - 2.0f*qz*qw, 2.0f*qx*qz + 2.0f*qy*qw, 0.0f,
2.0f*qx*qy + 2.0f*qz*qw, 1.0f - 2.0f*qx*qx - 2.0f*qz*qz, 2.0f*qy*qz - 2.0f*qx*qw, 0.0f,
2.0f*qx*qz - 2.0f*qy*qw, 2.0f*qy*qz + 2.0f*qx*qw, 1.0f - 2.0f*qx*qx - 2.0f*qy*qy, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f};
return matrix;
}
EDIT 2 :
I used glMultMatrixf() and it worked. But I finally found out, that the output of RotateCamera() makes my Quaternion zero ? Does anybody know whats wrong with this method :
void RotateCamera(double Angle, double x, double y, double z)
{
quaternion temp, quat_view, result;
temp.x = x * sin(Angle/2);
temp.y = y * sin(Angle/2);
temp.z = z * sin(Angle/2);
temp.w = cos(Angle/2);
quat_view.x = View.x;
quat_view.y = View.y;
quat_view.z = View.z;
quat_view.w = 0;
result = mult(mult(temp, quat_view), conjugate(temp));
View.x = result.x;
View.y = result.y;
View.z = result.z;
}
It doesn't really make sense to me , but I will try to answer anyway :D ... why don't you just rotate it using glRotatef(angle,0,0,1) for rotation of the z axis , since the the definition of this function it is as follows : glRotatef(angle,x_axis,y_axis,z_axis) the last 3 parameters clamp to [0,1].
For the second question , from what I know you should decrement the angle, you can anyway experiment with the function to see for yourself ;) .
I'm currently building a game where the played drives a semi truck and is followed and attacked by enemy cars by attempted slamming. I got some help from one of my teachers on how to get the enemy to choose which direction to go in order to follow and attack the player. Upon implementing what she gave me I got weird behavior and feel like I'm missing something.
When I place an enemy car in game near the player and pass the player's position into the function the enemy car simply spins in circles. If I add velocity to it i drives in large circles. Generally it never chooses a direction to drive straight in.
After debugging it seems like my if statement doesn't ever resolve and upon every update it keeps trying to get back towards 0 but for some reason it can't.
I am not sure if the coordinates of the player are creating the issue or if my math calculations or going wonky.
void EnemySpeedy::playerTracking(float posX, float posY)
{
//Direction choosing
dir.x = posX - pos.x;
dir.y = posY - pos.y;
//plus maybe this?
goalAngle = atan2f(dir.y, dir.x);
//I think this is the problem code?//
if (angle < goalAngle) angle -= sfw::getDeltaTime() * angularSpeed;
else angle += sfw::getDeltaTime() * angularSpeed;
//AI Movement alla adding velocity
acc = speed;
vel = vel + (acc - dragVel) * sfw::getDeltaTime();
vel = std::fmaxf(0, vel);
vel = std::fminf(vel, maxVel);
pos = { pos.x + vel * cosf(angle * PI / 180) * sfw::getDeltaTime(),
pos.y + vel * sinf(angle * PI / 180) * sfw::getDeltaTime() };
}
atan2f returns radians, so your goalAngle is in the range [-Pi,Pi].
I don't know if your angle and the angularSpeed use the same metric, but when you calculate the sinf and cosf you are converting angle from degrees to radians.
I suggest to keep all your angles in radians and check them:
#include <cmath>
inline float normAngle ( float ang ) {
return ang < -M_PI ? ang + 2.0*M_PI : ( ang > M_PI ? ang - 2.0*M_PI : ang);
}
inline float limitValue ( float x, float min, float max ) {
return x < min ? min : ( x > max ? max : x );
}
Then, you can try this logic:
void EnemySpeedy::playerTracking(float posX, float posY)
{
//Direction choosing, pos is a member of EnemySpeedy
float dirX = posX - pos.x;
float dirY = posY - pos.y;
//Angle choosing; angle, angularSpeed and angularSpeedMax are members of EnemySpeedy
float goalAngle = atan2(dirY, dirX);
float difAngle = normAngle(angle - goalAngle);
angularSpeed = limitValue(-difAngle,-angularSpeedMax,angularSpeedMax);
float dt = sfw::getDeltaTime();
angle = normAngle(angle + dt * angularSpeed);
// Update speed; acc, vel, etc. are members of EnemySpeedy class
// acc = speed; // it seems odd to me...
// vel = limitValue(vel + (acc - dragVel) * dt, 0.0, maxVel);
// what about:
acc = (difAngle > 1.5 || difAngle < -1.5) ? -maxAcc/2.0 : maxAcc*(maxVel - vel)/maxVel;
// brake if direction is wrong, go to limit velocity otherwise
acc = limitValue(acc, -maxAcc, maxAcc);
vel = limitValue(vel + acc * dt, 0.0, maxVel);
// Update position
pos.x += vel * cos(angle) * dt;
pos.y += vel * sin(angle) * dt;
}
I copied this ellipse code directly from the opengl textbook:
void ellipseMidpoint (int xCenter, int yCenter, int Rx, int Ry)
{
int Rx2 = Rx * Rx;
int Ry2 = Ry * Ry;
int twoRx2 = 2 * Rx2;
int twoRy2 = 2 * Ry2;
int p;
int x = 0;
int y = Ry;
int px = 0;
int py = twoRx2 * y;
//initial points in both quadrants
ellipsePlotPoints (xCenter, yCenter, x, y);
//Region 1
p = round (Ry2 - (Rx2 * Ry) + (0.25 * Rx2));
while (px < py) {
x++;
px += twoRy2;
if (p < 0)
p += Ry2 + px;
else {
y--;
py -= twoRx2;
p += Ry2 + px - py;
}
ellipsePlotPoints (xCenter, yCenter, x, y);
}
//Region 2
p = round (Ry2 * (x+0.5) * (x+0.5) + Rx2 * (y-1) * (y-1) - Rx2 * Ry2);
while (y > 0) {
y--;
py -= twoRx2;
if (p > 0)
p += Rx2 - py;
else {
x++;
px += twoRy2;
p += Rx2 - py + px;
}
ellipsePlotPoints (xCenter, yCenter, x, y);
}
}
void ellipsePlotPoints (int xCenter, int yCenter, int x, int y)
{
setPixel (xCenter + x, yCenter + y);
setPixel (xCenter - x, yCenter + y);
setPixel (xCenter + x, yCenter - y);
setPixel (xCenter - x, yCenter - y);
}
void setPixel (GLint xPos, GLint yPos)
{
glBegin (GL_POINTS);
glVertex2i(xPos, yPos);
glEnd();
}
The smaller ellipses seem to be fine but the larger ones are pointy and sort of flat at the ends.
Any ideas why?
Here is a current screenshot:
I think you're encountering overflow. I played with your code. While I never saw exactly the same "lemon" type shapes from your pictures, things definitely fell apart at large sizes, and it was caused by overflowing the range of the int variables used in the code.
For example, look at one of the first assignments:
int py = twoRx2 * y;
If you substitute, this becomes:
int py = 2 * Rx * Rx * Ry;
If you use a value of 1000 each for Rx and Ry, this is 2,000,000,000. Which is very close to the 2^31 - 1 top of the range of a 32-bit int.
If you want to use this algorithm for larger sizes, you could use 64-bit integer variables. Depending on your system, the type would be long or long long. Or more robustly, int64_t after including <stdint.h>.
Now, if all you want to do is draw an ellipsis with OpenGL, there are much better ways. The Bresenham type algorithms used in your code are ideal if you need to draw a curve pixel by pixel. But OpenGL is a higher level API, which knows how to render more complex primitives than just pixels. For a curve, you will most typically use a connected set of line segments to approximate the curve. OpenGL will then take care of turning those line segments into pixels.
The simplest way to draw an ellipsis is to directly apply the parametric representation. With phi an angle between 0 and PI, and using the naming from your code, the points on the ellipsis are:
x = xCenter + Rx * cos(phi)
y = yCenter + Ry * sin(phi)
You can use an increment for phi that meets your precision requirements, and the code will look something to generate an ellipsis approximated by DIV_COUNT points will look something like this:
float angInc = 2.0f * m_PI / (float)DIV_COUNT;
float ang = 0.0f;
glBegin(GL_LINE_LOOP);
for (int iDiv = 0; iDiv < DIV_COUNT; ++iDiv) {
ang += angInc;
float x = xCenter + Rx * cos(ang);
float y = yCenter + Ry * sin(ang);
glVertex2f(x, y);
glEnd();
If you care about efficiency, you can avoid calculating the trigonometric functions for each point, and apply an incremental rotation to calculate each point from the previous one:
float angInc = 2.0f * M_PI / (float)DIV_COUNT;
float cosInc = cos(angInc);
float sinInc = sin(angInc);
float cosAng = 1.0f;
float sinAng = 0.0f
glBegin(GL_LINE_LOOP);
for (int iDiv = 0; iDiv < DIV_COUNT; ++iDiv) {
float newCosAng = cosInc * cosAng - sinInc * sinAng;
sinAng = sinInc * cosAng + cosInc * sinAng;
cosAng = newCosAng;
float x = xCenter + Rx * cosAng;
float y = yCenter + Ry * sinAng;
glVertex2f(x, y);
glEnd();
This code is of course just for illustrating the math, and to get you started. In reality, you should use current OpenGL rendering methods, which includes vertex buffers, etc.
I'm working on some code to place isometric CCTMXTiledMap onto a CCLayerPanZoom control and then convert a touch location into ISO tilemap co-ordinates. This all works perfectly well for me, so long as the scale of the CClayerPanZoom is 1 (i.e. if I don't zoom in or zoom out). I can pan the map around and still calculate the correct iso tile co-oridinates. However, as soon as I zoom the tiled map in or out the iso cordinates returned by my code are completely wrong. Please see below for my code to calculate the iso co-ordinates from the touch location.
-(CGPoint) tilePosFromLocation:(CGPoint)location tileMap:(CCTMXTiledMap*)thisTileMap panZoom:(CCLayerPanZoom*)thisPanZoom
{
float midScale = (thisPanZoom.minScale + thisPanZoom.maxScale) / 2.0;
float newScale = (thisPanZoom.scale <= midScale) ? thisPanZoom.maxScale : thisPanZoom.minScale;
if (thisPanZoom.scale < 1)
{
newScale = newScale + thisPanZoom.scale;
}
else
{
newScale = newScale - thisPanZoom.scale;
}
CGFloat deltaX = (location.x - thisPanZoom.anchorPoint.x * (thisPanZoom.contentSize.width / CC_CONTENT_SCALE_FACTOR()) ) * (newScale);
CGFloat deltaY = (location.y - thisPanZoom.anchorPoint.y * (thisPanZoom.contentSize.height / CC_CONTENT_SCALE_FACTOR()) ) * (newScale);
CGPoint position = ccp((thisPanZoom.position.x - deltaX) , (thisPanZoom.position.y - deltaY) );
float halfMapWidth = thisTileMap.mapSize.width * 0.5f;
float mapHeight = thisTileMap.mapSize.height;
float tileWidth = thisTileMap.tileSize.width / CC_CONTENT_SCALE_FACTOR() * newScale;
float tileHeight = thisTileMap.tileSize.height / CC_CONTENT_SCALE_FACTOR() * newScale;
CGPoint tilePosDiv = CGPointMake(position.x / tileWidth, position.y / tileHeight );
float inverseTileY = tilePosDiv.y - (mapHeight * CC_CONTENT_SCALE_FACTOR()) * newScale; //mapHeight + tilePosDiv.y;
float posX = (int)(tilePosDiv.y - tilePosDiv.x + halfMapWidth);
float posY = (int)(inverseTileY + tilePosDiv.x - halfMapWidth + mapHeight);
// make sure coordinates are within isomap bounds
posX = MAX(0, posX);
posX = MIN(thisTileMap.mapSize.width - 1, posX);
posY = MAX(0, posY);
posY = MIN(thisTileMap.mapSize.height - 1, posY);
return CGPointMake(posX, posY);
}
Can anyone offer any insight into where I'm going wrong with this?
Thanks,
Alan
I'm doing some specific signal analysis, and I am in need of a method that would smooth out a given bell-shaped distribution curve. A running average approach isn't producing the results I desire. I want to keep the min/max, and general shape of my fitted curve intact, but resolve the inconsistencies in sampling.
In short: if given a set of data that models a simple quadratic curve, what statistical smoothing method would you recommend?
If possible, please reference an implementation, library, or framework.
Thanks SO!
Edit: Some helpful data
(A possible signal graph)
The dark colored quadratic is my "fitted" curve of the light colored connected data points.
The sample # -44 (approx.), is a problem in my graph (i.e. a potential sample inconsistency). I need this curve to "fit" the distribution better, and overcome the values that do not trend accordingly. Hope this helps!
A "quadratic" curve is one thing; "bell-shaped" usually means a Gaussian normal distribution. Getting a best-estimate Gaussian couldn't be easier: you compute the sample mean and variance and your smooth approximation is
y = exp(-squared(x-mean)/variance)
If, on the other hand, you want to approximate a smooth curve with a quadradatic, I'd recommend computing a quadratic polynomial with minimum square error. I can nenver remember the formulas for this, but if you've had differential calculus, write the formula for the total square error (pointwise) and differentiate with respect to the coefficients of your quadratic. Set the first derivatives to zero and solve for the best approximation. Or you could look it up.
Finally, if you just want a smooth-looking curve to approximate a set of points, cubic splines are your best bet. The curves won't necessarily mean anything, but you'll get a nice smooth approximation.
#include <iostream>
#include <math.h>
struct WeightedData
{
double x;
double y;
double weight;
};
void findQuadraticFactors(WeightedData *data, double &a, double &b, double &c, unsigned int const datasize)
{
double w1 = 0.0;
double wx = 0.0, wx2 = 0.0, wx3 = 0.0, wx4 = 0.0;
double wy = 0.0, wyx = 0.0, wyx2 = 0.0;
double tmpx, tmpy;
double den;
for (unsigned int i = 0; i < datasize; ++i)
{
double x = data[i].x;
double y = data[i].y;
double w = data[i].weight;
w1 += w;
tmpx = w * x;
wx += tmpx;
tmpx *= x;
wx2 += tmpx;
tmpx *= x;
wx3 += tmpx;
tmpx *= x;
wx4 += tmpx;
tmpy = w * y;
wy += tmpy;
tmpy *= x;
wyx += tmpy;
tmpy *= x;
wyx2 += tmpy;
}
den = wx2 * wx2 * wx2 - 2.0 * wx3 * wx2 * wx + wx4 * wx * wx + wx3 * wx3 * w1 - wx4 * wx2 * w1;
if (den == 0.0)
{
a = 0.0;
b = 0.0;
c = 0.0;
}
else
{
a = (wx * wx * wyx2 - wx2 * w1 * wyx2 - wx2 * wx * wyx + wx3 * w1 * wyx + wx2 * wx2 * wy - wx3 * wx * wy) / den;
b = (-wx2 * wx * wyx2 + wx3 * w1 * wyx2 + wx2 * wx2 * wyx - wx4 * w1 * wyx - wx3 * wx2 * wy + wx4 * wx * wy) / den;
c = (wx2 * wx2 * wyx2 - wx3 * wx * wyx2 - wx3 * wx2 * wyx + wx4 * wx * wyx + wx3 * wx3 * wy - wx4 * wx2 * wy) / den;
}
}
double findY(double const a, double const b, double const c, double const x)
{
return a * x * x + b * x + c;
};
int main(int argc, char* argv[])
{
WeightedData data[9];
data[0].weight=1; data[0].x=1; data[0].y=-52.0;
data[1].weight=1; data[1].x=2; data[1].y=-48.0;
data[2].weight=1; data[2].x=3; data[2].y=-43.0;
data[3].weight=1; data[3].x=4; data[3].y=-44.0;
data[4].weight=1; data[4].x=5; data[4].y=-35.0;
data[5].weight=1; data[5].x=6; data[5].y=-31.0;
data[6].weight=1; data[6].x=7; data[6].y=-32.0;
data[7].weight=1; data[7].x=8; data[7].y=-43.0;
data[8].weight=1; data[8].x=9; data[8].y=-52.0;
double a=0.0, b=0.0, c=0.0;
findQuadraticFactors(data, a, b, c, 9);
std::cout << " x \t y" << std::endl;
for (int i=0; i<9; ++i)
{
std::cout << " " << data[i].x << ", " << findY(a,b,c,data[i].x) << std::endl;
}
}
How about a simple digital low-pass filter?
y[0] = x[0];
for (i = 1; i < len; ++i)
y[i] = a * x[i] + (1.0 - a) * y[i - 1];
In this case, x[] is your input data and y[] is the filtered output. The a coefficient is a value between 0 and 1 that you should tweak. An a value of 1 reproduces the input and the cut-off frequency decreases as a approaches 0.
Perhaps the parameters for your running average are set wrong (sample window too small or large)?
Is it just noise superimposed on your bell curve? How close is the noise frequency to that of the signal you're trying to retrieve? A picture of what you're trying to extract might help us identify a solution.
You could try some sort of fitting algorithm using a least squares fit if you can make a reasonable guess of the function parameters. Those sorts of techniques often have some immunity to noise.