Vector values becoming NaN - c++

I keep getting nan (not a number) as a result for the various floats and vector components throughout my program. I am almost 100% certain is has to do with collision because this was never an issue before I messed with the collision code.
Even the sum in the magnitude method is showing up as NaN when I debug it, which leads me to believe a vector is breaking before being passed into the function, but I cannot find which one it is.
Here are the methods that I believe are relevant to the problem
void Simplex::PhysicsInfo::Collision(PhysicsInfo info)
{
vector3 oldVel = velocity;
if (magnitude(oldVel) == 0.0f)
{
return;
}
vector3 nextVelDirect = glm::normalize(info.position - position);
//make all ball to ball collisions elastic
float angle = acosf(glm::dot(oldVel, nextVelDirect)
/ (magnitude(glm::normalize(oldVel)) * magnitude(nextVelDirect)));
angle = sinf(angle);
if (angle < 0)
angle *= -1;
float nextVecMag;
if (magnitude(info.velocity) == 0 && angle != 1)
{
//This next line is not correct, use if actual line isn't working and you absolutely need something
//info.velocity = 0.5f * oldVel.length * nextVelDirect;
//actual line
info.velocity = angle * magnitude(oldVel) * nextVelDirect;
vector3 nextVec = (magnitude(oldVel) * oldVel)
- (magnitude(info.velocity) * info.velocity);
nextVecMag = magnitude(nextVec);
if (nextVecMag < 0)
{
nextVecMag *= -1;
}
nextVecMag = sqrt(nextVecMag);
velocity = nextVecMag * glm::normalize(nextVec);
}
else if (magnitude(info.velocity) == 0)
{
info.velocity = oldVel;
velocity = vector3(0.0f);
}
if (isnan(velocity.x) || isnan(velocity.y) || isnan(velocity.z))
{
std::cout << "-" << std::endl;
}
}
PhysicsInfo::PhysicsInfo(float mss, vector3 pos, vector3 cent, vector3 limit)
{
velocity = vector3(0.1f);
acceleration = vector3(0.0f);
mass = mss;
position = pos;
center = cent;
limits = limit;
frictionMagnitude = 0.005f;
}
vector3 PhysicsInfo::normalize(const vector3 &v)
{
float sum = (v.x * v.x) + (v.y * v.y) + (v.z * v.z);
if (sum < 0)
{
sum *= -1;
}
float length_of_v = sqrt(sum);
return vector3(v.x / length_of_v, v.y / length_of_v, v.z / length_of_v);
}
float PhysicsInfo::magnitude(const vector3 &v)
{
float sum = (v.x * v.x) + (v.y * v.y) + (v.z * v.z);
if (sum < 0)
{
sum *= -1;
}
float length_of_v = sqrt(sum);
if (isnan(length_of_v))
{
throw ExceptionCollidedUnwind;
}
return length_of_v;
}
Sorry for the formatting. I am not used to posting here. Any help would be greatly appreciated.

I have not fixed the issue yet, but I have deciphered that the cause was the parameter to the acosf() function located the the Collision method. The dot method sometimes returns outside of the required range of [-1, 1], the required range of floats for the parameter to the acosf() method. This was caused because I did not normalize both vectors before getting the dot product.

Related

Quaternion slerp with shortest path not working

My slerp routine is below. From what I've read, the check against > 0 should handle it so it always takes the shortest path. But it never does. In the case where I cross a "pole", the quaternion flips out and produces angles with NAN values.
quat quat::slerp(quat dest, float t)
{
const quat &from = *this;
static const double epsilon = 0.0001;
double theta, cosTheta, sinTheta;
double p, q;
cosTheta = from.x*dest.x + from.y*dest.y + from.z*dest.z + from.w*dest.w;
if(cosTheta < 0.0)
{
dest = { -from.x, -from.y, -from.z, -from.w };
cosTheta = -cosTheta;
}
if((1.0-fabs(cosTheta)) > epsilon)
{
theta = acos(cosTheta);
sinTheta = sin(theta);
q = sin((1-t) * theta) / sinTheta;
p = sin(t*theta) / sinTheta;
}
else
{
q = 1-t;
p = t;
}
quat qo;
qo.w = (float)((q * from.w) + (p * dest.w));
qo.x = (float)((q * from.x) + (p * dest.x));
qo.y = (float)((q * from.y) + (p * dest.y));
qo.z = (float)((q * from.z) + (p * dest.z));
return qo;
}
Maybe there are other bugs as well, but this line certainly has one:
dest = { -from.x, -from.y, -from.z, -from.w };
It overwrites dest with -from, which is not correct. It should be:
dest = { -dest.x, -dest.y, -dest.z, -dest.w };

Ray tracing sphere reflection bug

I am trying to implement the ray tracing algorithm and I have some trouble computing the reflected rays of spherical objects.It seems that
for some particular rays, the reflected ray just passes through and is collinear with the traced ray.
Bellow is how i record the ray - sphere intersection:
bool Sphere::intersectLocal(const ray & r, isect & i) const {
Vec3d P = r.getPosition();
Vec3d D = r.getDirection();
//D.normalize();
double a = dot(D, D);
double b = 2 * dot(P, D);
double c = dot(P, P) - 1;
double delta = b * b - 4 * a * c;
if (delta < 0)
return false;
if (delta == 0) {
double t = -b / 2 * a;
Vec3d Q = P + t * D;
Vec3d N = Q;
N.normalize();
i.setT(t);
i.setN(N);
i.setObject(this);
return true;
}
if (delta > 0) {
double t1 = (-b - sqrt(delta)) / 2 * a;
double t2 = (-b + sqrt(delta)) / 2 * a;
double t;
if (t1 > 0) t = t1;
else if (t2 > 0) t = t2;
else return false;
Vec3d N = P + t * D;
N.normalize();
i.setT(t);
i.setN(N);
i.setObject(this);
return true;
}
return false;
}
And this is how I compute the reflected ray for each intersection:
isect i;
if (scene - > intersect(r, i)) {
// An intersection occured!
const Material & m = i.getMaterial();
double t = i.t;
Vec3d N = i.N;
Vec3d I = m.shade(scene, r, i); //local illumination
if (!m.kr(i).iszero() && depth >= 0) {
// compute reflection direction
Vec3d raydir = r.getDirection();
Vec3d refldir = 2 * dot(-raydir, i.N) * i.N + raydir;
refldir.normalize();
ray reflectionRay = ray(r.at(i.t), refldir, ray::RayType::REFLECTION);
Vec3d reflection = traceRay(reflectionRay, thresh, depth - 1);
Vec3d R = reflection;
I += R;
}
return I;
} else {
// No intersection. This ray travels to infinity, so we color
// it according to the background color, which in this (simple) case
// is just black.
return Vec3d(0.0, 0.0, 0.0);
}
The code above seems to work fine for most of the points on the sphere where the rays intersect, but for others it does not reflect as i expected
If I see right, this makes the normal face same direction as the ray. So with ray==normal==reflected_ray nothing gets reflected.
Vec3d Q = P + t * D;
Vec3d N = Q;
About errors in floating-point arithmetic and how to deal with it:
What Every Computer Scientist Should Know About Floating-Point Arithmetic
Here you can find how to compare floating-point numbers. Searching for relative absolute compare floating you may find more information.
https://floating-point-gui.de/errors/comparison/
This is an excerpt from my code in C#. Almost never use absolute compare.
public static bool IsAlmostRelativeEquals(this double d1, double d2, double epsilon)
{
double absDiff = Math.Abs(d1 - d2);
if (double.IsPositiveInfinity(absDiff))
return false;
if (absDiff < epsilon)
return true;
double absMax = Math.Max(Math.Abs(d1), Math.Abs(d2));
return Math.Abs(d1 - d2) <= epsilon * absMax;
}
public static bool IsAlmostZero(this double d, double epsilon)
{
double abs = Math.Abs(d);
if (double.IsPositiveInfinity(abs))
return false;
return abs < epsilon;
}

How to extract variables from a equation?

Im studying some code and I would like help with some math. Im trying to solve the equation of the tangent line on a circle with given point of tangency.
//(x1 - p)(x - p) +(y1 - q)(y - q) = r^2 I understand this formula
//variables
//x1 = point.x
//y1 = point.y
//p = center.x
//q = center.y
//r = radius
edit: here is the whole function, maybe it will help. My teacher gave it to me to study, but maybe he is trolling me :D
const std::pair<double, double> Arc::tangentEquation(const glm::vec3& center, const glm::vec3& pointA, float radius) const {
if (radius <= 0.0f)
throw std::domain_error("Radius can't be negative or 0");
// Jednadžba tangente u točki T
// (x1 - p)(x - p) + (y1 - q)(y - q) = r^2
glm::vec3 point = pointA + center;
double px = -1 * (center.x * point.x);
double qy = -1 * (center.y * point.y);
double x = point.x - center.x;
double y = point.y - center.y;
double k = 0.0;
double l = (pow(radius, 2) - (px + pow(center.x, 2) + qy + pow(center.y, 2)));
if (y == 0) { // paralelan s x os
k = l / x;
l = 0;
} else if (x == 0) { // paralelan s y os
l = l / y;
k = 0;
} else {
k = -x / y;
l = l / y;
}
return std::pair<double, double>(k, l);
}
The code does not implement the formula on the first line, so I don't think it is strange that you don't understand :-)
(x1 - p)(x - p) + (y1 - q)(y - q)
If we write out all the terms in the parenthesis multiplication, we get:
x1*x - p*x - p*x1 + p^2 + y1*y - q*y - q*y1 + q^2
(https://www.youtube.com/watch?v=3s_lroR5_1U for very pedagogic explanation)
But your code looses half of these terms....?

Continous angles in C++ (eq unwrap function in matlab)

I guess it is not that hard, but I have been stuck on that one for a while.
I have a joint that can rotate both direction. A sensor gives me the angle of the joint in the range -pi and +pi.
I would like to convert it in the range -infinity and +infinity. Meaning that if for example the joint rotate clockwise forever, the angle would start at 0 and then increase to infinity.
In matlab, the unwrap function does that very well:
newAngle = unwrap([previousAngle newAngle]);
previousAngle = newAngle;
Note: it is assumed the angle does not make big jump, nothing superior to PI for sure.
Note: I really looked hard before asking ...
Thanks !
The following function does the job, assuming the absolute difference between the input angles is less than 2*pi:
float unwrap(float previous_angle, float new_angle) {
float d = new_angle - previous_angle;
d = d > M_PI ? d - 2 * M_PI : (d < -M_PI ? d + 2 * M_PI : d);
return previous_angle + d;
}
If you need to unwrap an array, you can use this routine:
void unwrap_array(float *in, float *out, int len) {
out[0] = in[0];
for (int i = 1; i < len; i++) {
float d = in[i] - in[i-1];
d = d > M_PI ? d - 2 * M_PI : (d < -M_PI ? d + 2 * M_PI : d);
out[i] = out[i-1] + d;
}
}
After some work, came up with this. Seems to be working fine.
//Normalize to [-180,180):
inline double constrainAngle(double x){
x = fmod(x + M_PI,M_2PI);
if (x < 0)
x += M_2PI;
return x - M_PI;
}
// convert to [-360,360]
inline double angleConv(double angle){
return fmod(constrainAngle(angle),M_2PI);
}
inline double angleDiff(double a,double b){
double dif = fmod(b - a + M_PI,M_2PI);
if (dif < 0)
dif += M_2PI;
return dif - M_PI;
}
inline double unwrap(double previousAngle,double newAngle){
return previousAngle - angleDiff(newAngle,angleConv(previousAngle));
}
I used code from this post:
Dealing with Angle Wrap in c++ code
// wrap to [-pi,pi]
inline double angle_norm(double x)
{
x = fmod(x + M_PI, M_2PI);
if (x < 0)
x += M_2PI;
return x - M_PI;
}
double phase_unwrap(double prev, double now)
{
return prev + angle_norm(now - prev);
}
This works.

When moving something to a point with vector math the enemy gets there and then disappears

I have been trying to use vectors to move objects at angles and I did get it working, however, when I try to move an object to a specific point it gets there and then disappears. In my code I test if within the next step if it will reach it's destination and if it will, I snap it to the destination.
void Dot::moveToVector(Vector& vec)
{
float dx;
float dy;
dx = vec.X - position.X;
dy = vec.Y - position.Y;
Vector distanceVec(dx, dy);
float distance = distanceVec.Length();
float scale;
scale = speed / distance;
velocity.X = dx * scale;
velocity.Y = dy * scale;
if(velocity.X < scale || velocity.Y < scale)
{
velocity.X = 0;
velocity.Y = 0;
position.X = vec.X;
position.Y = vec.Y;
}
move();
}
When I debugged it, one frame after it snaps into position, the x and y values of the position = -nan(0x400000).
scale = speed / distance;
If distance == 0 what do you think will happen?
When your object reaches the target position, distance becomes zero. Then you are dividing by distance. I suspect that is why your object disappears!
Here is a more straightforward way to set it up:
void Dot::moveToVector(Vector& vec)
{
Vector distanceVec = vec - position;
float distance = distanceVec.Length();
if(distance <= speed)
{
velocity.X = 0;
velocity.Y = 0;
position.X = vec.X;
position.Y = vec.Y;
}
else
{
Vector direction = (distanceVec / distance);
velocity = direction * speed;
}
move();
}