I'm trying to find the midpoint of a two 3D vectors (A and B). I believe it should be VectorA + VectorB/2 will give the midpoint. But how would I use create a fucntion that returns the midpoint like this V3D Midpoint(V3D A, V3D B); if I have a class like this:
class V3D
{
public:
V3D();
V3D(float x, float y, float z);
~V3D();
V3D operator+( const V3D& V ) const;
V3D operator(const V3D& V ) const;
V3D operator*( float Scale ) const;
V3D operator/( FLOAT Scale ) const;
float dot( const V3D& V ) const;
V3D cross( const V3D& V ) const;
V3D Normal() const; // this vector's unit vector
float Size() const; // magnitude of this vector
private:
float X, Y, Z;
};
Doesn't this work ?
V3D Midpoint(const V3D& A, const V3D& B)
{
return (A + B) / 2.0;
}
This is simply writing in code what you specified in the question.
Related
I am using Ceres Solver to perform non-linear curve fits on small data sets. Following the examples I am able to generate perfectly reasonable fit parameters for models that match my data well. I am also trying to compute the parameter variances and this is where things are falling apart. The code executes but results seem incorrect, often many orders of magnitude larger than the the parameter itself. The number of points (x, y) in the data sets I am fitting is similar to the number of parameter in the fit models, e.g. 4 data points, 3 parameters.
I came across a similar SO question here: Ceres: Compute uncertainty on parameter, which was helpful in linking the Ceres wiki on using the covariance class, but the issue was not marked as resolved. I, like the previous poster, looked at the parameter variances produced using the Python lmfit (https://lmfit.github.io/lmfit-py/index.html) package and found that it provides much more reasonable results.
The Ceres description of the covariance class (http://ceres-solver.org/nnls_covariance.html#example-usage) described a potential issue where if the residuals of the cost functor are not scaled correctly, i.e. by the positive semi-definite covariance matrix of the observed data, then the parameter covariance matrix computation can't be trusted. Not being a mathematician, I am not certain how to satisfy this requirement.
Below is some sample code showing the cost function that I've implemented as well as the usage of the covariance class. Any advice would be greatly appreciated.
Cost function:
struct BiExponential1 {
BiExponential1( double x, double y, double s ) : x_( x ), y_( y ), s_( s ) {}
template <typename T>
bool operator()( const T* const a, const T* const b, const T* const c, T* residual ) const {
residual[0] = y_ - a[0] * ( exp( -b[0] * x_ ) - exp( -c[0] * x_ ) ); // observed - estimated = y - ( a' [exp( -b' * x ) - exp(-c' * x)] )
return true;
}
private:
const double x_;
const double y_;
};
Solver/Covariance usage:
double a = init_value_a;
double b = init_value_b;
double c = init_value_c;
double data_x[nData] = {<dummy data>}
double data_y[nData] = {<dummy data>}
for ( int i = 0; i < nData; ++i ) {
problem.AddParameterBlock( &a, 1 );
problem.AddParameterBlock( &b, 1 );
problem.AddParameterBlock( &c, 1 );
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<BiExponential1, 1, 1, 1, 1>(
new BiExponential1( data_x[i], data_y[i] ) ),
nullptr,
&a,
&b,
&c );
}
// Run the solver and record the results.
ceres::Solve( solverOptions, &problem, &summary );
// Variance estimates
// Code adapted from: http://ceres-solver.org/nnls_covariance.html#example-usage
Covariance::Options covOptions;
// TESTED non-default algorithm type - no effect.
//covOptions.algorithm_type = ceres::CovarianceAlgorithmType::DENSE_SVD;
Covariance covariance( covOptions );
std::vector<std::pair<const double*, const double*> > covariance_blocks;
covariance_blocks.push_back( std::make_pair( &a, &a ) );
covariance_blocks.push_back( std::make_pair( &b, &b ) );
covariance_blocks.push_back( std::make_pair( &c, &c ) );
covariance_blocks.push_back( std::make_pair( &a, &b ) );
covariance_blocks.push_back( std::make_pair( &a, &c ) );
covariance_blocks.push_back( std::make_pair( &b, &c ) );
CHECK( covariance.Compute( covariance_blocks, &problem ) );
// Get the diagonal variance terms
double covariance_aa[1 * 1];
double covariance_bb[1 * 1];
double covariance_cc[1 * 1];
covariance.GetCovarianceBlock( &a, &a, covariance_aa );
covariance.GetCovarianceBlock( &b, &b, covariance_bb );
covariance.GetCovarianceBlock( &c, &c, covariance_cc );
I'd like to know if it is worth it optimizing my Vector3 class' operations with neon/simd like I did to my Vector2 class.
As far as I know, simd can only handle two or four floats at the same time, so to my Vector3 we would need something like this:
Vector3 Vector3::operator * (const Vector3& v) const
{
#if defined(__ARM_NEON__)
// extra step: allocate a fourth float
const float v4A[4] = {x, y, z, 0};
const float v4B[4] = {v.x, v.y, v.z, 0};
float32x4_t r = vmul_f32(*(float32x4_t*)v4A, *(float32x4_t*)v4B);
return *(Vector3*)&r;
#else
return Vector3(x * v.x, y * v.y, z * v.z);
#endif
}
Is this safe? Would this extra step still be faster than a non-simd code on most scenarios (say arm64 for instance)?
I'm trying to interpolate a 2D angle and it works 99.9% of the time. For some reason I'm getting -nan(ind) for some values, like:
lastAngle = -0.0613451
currentAngle = -0.061421
alpha = 0.218813
This is the code:
inline float slerpRotation(const float& angle1, const float& angle2, const float& alpha)
{
auto v1 = b2Vec2{std::cos(angle1), std::sin(angle1)};
auto v2 = b2Vec2{std::cos(angle2), std::sin(angle2)};
auto v = this->slerp(v1, v2, alpha);
return std::atan2(v.y, v.x);
}
inline b2Vec2 slerp(const b2Vec2& v1, const b2Vec2& v2, const float& alpha)
{
auto cosAngle = v1.x * v2.x + v1.y * v2.y;
auto angle = std::acos(cosAngle);
auto angleAlpha = angle * alpha;
auto v3 = (v2 - (cosAngle * v1)).Normalize();
auto x = v1.x * std::cos(angleAlpha) + v3 * std::sin(angleAlpha);
auto y = v1.y * std::cos(angleAlpha) + v3 * std::sin(angleAlpha);
return b2Vec2{x, y};
}
All this examples results in inf num:
slerpRotation(-0.0613451f, -0.061421f, 0.218813f);
slerpRotation(-1.63139f, -1.63139f, 0.723703f);
slerpRotation(-0.0614404f, -0.0614034f, 0.199831f);
slerpRotation(0.0194162f, 0.0194164f, 0.259074f);
I've tried to solve this problem for a while now without knowing what causes this problem, do you guys happened to know how to solve this?
In the end you are computing
angle1+alpha*(angle2-angle1)
or if you want to exclude some fringe cases,
angle1+alpha*reduce2pi(angle2-angle1)
where
reduce2pi(phi) = fmod( 3*pi + fmod(phi, 2*pi), 2*pi)-pi
Note that these formulas are completely singularity free, as there is no division. It is not necessary to switch forth and back between angles and their point on the unit circle.
In code, that would be
inline float slerpRotation(const float& angle1, const float& angle2, const float& alpha)
{
auto angleDiff = angle2-angle1;
angleDiff = std::fmod(angleDiff, 2*std::M_PI);
angleDiff = std::fmod(angleDiff + 3*std::M_PI, 2*std::M_PI)-std::M_PI;
return angle1+alpha*angleDiff;
}
(12/13/2016) combining several comments: If you insist on using exactly this interface structure, then you can get a singularity free method as follows:
inline b2Vec2 slerp(const b2Vec2& v1, const b2Vec2& v2, const float& alpha)
{
auto angle = std::atan2(v1.x*v2.y - v1.y*v2.x, v1.x*v2.x + v1.y*v2.y);
auto angleAlpha = angle * alpha;
auto v3=b2Vec2{-v1.y, v1.x}; // rotation by 90°
return std::cos(angleAlpha)*v1 + std::sin(angleAlpha)*v3;
}
I'm making a application for school in which I have to click a particular object.
EDIT: This is being made in 2D
I have a rectangle, I rotate this rectangle by X.
The rotation of the rectangle has made my rectangles (x,y,width,height) become a new rectangle around the rotated rectangle.
http://i.stack.imgur.com/MejMA.png
(excuse me for my terrible paint skills)
The Black lines describe the rotated rectangle, the red lines are my new rectangle.
I need to find out if my mouse is within the black rectangle or not. Whatever rotation I do I already have a function for getting the (X,Y) for each corner of the black rectangle.
Now I'm trying to implement this Check if point is within triangle (The same side technique).
So I can either check if my mouse is within each triangle or if theres a way to check if my mouse is in the rotated rectangle that would be even better.
I practically understand everything written in the triangle document, but I simply don't have the math skills to calculate the cross product and the dot product of the 2 cross products.
This is supposed to be the cross product:
a × b = |a| |b| sin(θ) n
|a| is the magnitude (length) of vector a
|b| is the magnitude (length) of vector b
θ is the angle between a and b
n is the unit vector at right angles to both a and b
But how do I calculate the unit vector to both a and b?
And how do I get the magnitude of a vector?
EDIT:
I forgot to ask for the calculation of the dotproduct between 2 cross products.
function SameSide(p1,p2, a,b)
cp1 = CrossProduct(b-a, p1-a)
cp2 = CrossProduct(b-a, p2-a)
if DotProduct(cp1, cp2) >= 0 then return true
else return false
Thank you everyone for your help I think I got the hang of it now, I wish I could accept multiple answers.
If you are having to carry out loads of check, I would shy away from using square root functions: they are computationally expensive. for comparison purposes, just multiply everything by itself and you can bypass the square rooting:
magnitude of vector = length of vector
If vector is defined as float[3] length can be calculated as follows:
double magnitude = sqrt( a[0]*a[0] + a[1]*a[1] + a[2]*a[2] );
However that is expensive computationally so I would use
double magnitudeSquared = a[0]*a[0] + a[1]*a[1] + a[2]*a[2];
Then modify any comparative calculations to use the squared version of the distance or magnitude and it will be more performant.
For the cross product, please forgive me if this maths is shaky, it has been a couple of years since I wrote functions for this (code re-use is great but terrible for remembering things):
double c[3];
c[0] = ( a[1]*b[2] - a[2]*b[1] );
c[1] = ( a[2]*b[0] - a[0]*b[2] );
c[2] = ( a[0]*b[1] - a[1]*b[0] );
To simplify it all I would put a vec3d in a class of its own, with a very simple representation being:
class vec3d
{
public:
float x, y, z;
vec3d crossProduct(vec3d secondVector)
{
vec3d retval;
retval.x = (this.y * secondVector.z)-(secondVector.y * this.z);
retval.y = -(this.x * secondVector.z)+(secondVector.x * this.z);
retval.z = (this.x * secondVector.y)-(this.y * secondVector.x);
return retval;
}
// to get the unit vector divide by a vectors length...
void normalise() // this will make the vector into a 1 unit long variant of itself, or a unit vector
{
if(fabs(x) > 0.0001){
x= x / this.magnitude();
}
if(fabs(y) > 0.0001){
y= y / this.magnitude();
}
if(fabs(z) > 0.0001){
z = / this.magnitude();
}
}
double magnitude()
{
return sqrt((x*x) + (y*y) + (z*z));
}
double magnitudeSquared()
{
return ((x*x) + (y*y) + (z*z));
}
};
A fuller implementation of a vec3d class can be had from one of my old 2nd year coding excercises: .h file and .cpp file.
And here is a minimalist 2d implementation (doing this off the top of my head so forgive the terse code please, and let me know if there are errors):
vec2d.h
#ifndef VEC2D_H
#define VEC2D_H
#include <iostream>
using namespace std;
class Vec2D {
private:
double x, y;
public:
Vec2D(); // default, takes no args
Vec2D(double, double); // user can specify init values
void setX(double);
void setY(double);
double getX() const;
double getY() const;
double getMagnitude() const;
double getMagnitudeSquared() const;
double getMagnitude2() const;
Vec2D normalize() const;
double crossProduct(Vec2D secondVector);
Vec2D crossProduct(Vec2D secondVector);
friend Vec2D operator+(const Vec2D&, const Vec2D&);
friend ostream &operator<<(ostream&, const Vec2D&);
};
double dotProduct(const Vec2D, const Vec2D);
#endif
vec2d.cpp
#include <iostream>
#include <cmath>
using namespace std;
#include "Vec2D.h"
// Constructors
Vec2D::Vec2D() { x = y = 0.0; }
Vec2D::Vec2D(double a, double b) { x = a; y = b; }
// Mutators
void Vec2D::setX(double a) { x = a; }
void Vec2D::setY(double a) { y = a; }
// Accessors
double Vec2D::getX() const { return x; }
double Vec2D::getY() const { return y; }
double Vec2D::getMagnitude() const { return sqrt((x*x) + (y*y)); }
double Vec2D::getMagnitudeSquared() const { return ((x*x) + (y*y)); }
double Vec2D::getMagnitude2 const { return getMagnitudeSquared(); }
double Vec2d::crossProduct(Vec2D secondVector) { return ((this.x * secondVector.getY())-(this.y * secondVector.getX()));}
Vec2D crossProduct(Vec2D secondVector) {return new Vec2D(this.y,-(this.x));}
Vec2D Vec2D::normalize() const { return Vec2D(x/getMagnitude(), y/getMagnitude());}
Vec2D operator+(const Vec2D& a, const Vec2D& b) { return Vec2D(a.x + b.x, a.y + b.y);}
ostream& operator<<(ostream& output, const Vec2D& a) { output << "(" << a.x << ", " << a.y << ")" << endl; return output;}
double dotProduct(const Vec2D a, const Vec2D b) { return a.getX() * b.getX() + a.getY() * b.getY();}
Check if a point is inside a triangle described by three vectors:
float calculateSign(Vec2D v1, Vec2D v2, Vec2D v3)
{
return (v1.getX() - v3.getX()) * (v2.getY() - v3.getY()) - (v2.getX() - v3.getX()) * (v1.getY() - v3.getY());
}
bool isPointInsideTriangle(Vec2D point2d, Vec2D v1, Vec2D v2, Vec2D v3)
{
bool b1, b2, b3;
// the < 0.0f is arbitrary, could have just as easily been > (would have flipped the results but would compare the same)
b1 = calculateSign(point2d, v1, v2) < 0.0f;
b2 = calculateSign(point2d, v2, v3) < 0.0f;
b3 = calculateSign(point2d, v3, v1) < 0.0f;
return ((b1 == b2) && (b2 == b3));
}
In the code above if calculateSign is in the triangle you will get a true returned :)
Hope this helps, let me know if you need more info or a fuller vec3d or 2d class and I can post:)
Addendum
I have added in a small 2d-vector class, to show the differences in the 2d and 3d ones.
The magnitude of a vector is its length. In C++, if you have a vector represented as a double[3], you would calculate the length via
#include <math.h>
double a_length = sqrt( a[0]*a[0] + a[1]*a[1] + a[2]*a[2] );
However, I understand what you actually want is the cross product? In that case, you may want to calculate it directly. The result is a vector, i.e. c = a x b.
You code it like this for example:
double c[3];
c[0] = ( a[2]*b[3] - a[3]*b[2] );
c[1] = ( a[3]*b[1] - a[1]*b[3] );
c[2] = ( a[1]*b[2] - a[2]*b[1] );
You can calculate the magnitude of vector by sqrt(x*x + y*y). Also you can calculate the crossproduct simpler: a x b = a.x * b.y - a.y * b.x. Checking that a point is inside triangle can be done by counting the areas for all 4 triangles. For example a is the area of the source triangle, b,c,d are areas of other ones. If b + c + d = a then the point is inside. Counting the area of triangle is simple: we have vectors a, b that are vertexes of triangle. The area of triangle then is (a x b) / 2
One simple way without getting into vectors is to check for area.
For example ,lets say you have a rectangle with corners A,B,C,D. and point P.
first calculate the area of rectangle, simply find height and width of the rectangle and multiply.
B D
| /
| /
|/____ C
A
For calculating the height,width take one point lets say A, find its distance from all other three points i.e AB,AC,AD 1st and 2nd minimum will be width,and height, max will be diagonal length.
Now store the points from which you get the height, width, lets says those points are B,C.
So now you know how rectangle looks, i.e
B _____ D
| |
|_____|
A C
Then calculate the sum of area of triangles ACP,ABP,BDP,CDP (use heros formula to compute area of rectangle), if it equals to the area of rectangle, point P is inside else outside the rectangle.
What I have to do is create a square that is made up of 8 triangles, all the same size, using arrays. The coordinates of the four corners of the square are, (-10, -10, 10), (-10, -10, -10), (10, -10, -10), (10, -10, 10). And that's starting with the upper left and going counter clockwise.
I have already created it before just entering values into the array but now I have to figure out how to do it using for loops in C++. So I know that for each array (I need to create a vertex, index and color array) I need to create a for loop and that that for loop has to have a for loop inside of it.
I like to use Eigen::Vector2f for Vec but anything with a similar interface should work:
template< typename Vec >
void glVec2d( const Vec& vec )
{
glVertex2d( vec.x(), vec.y() );
}
template< typename Vec >
void glTex2d( const Vec& vec )
{
glTexCoord2d( vec.x(), vec.y() );
}
template< typename Vec >
void glQuad2d
(
const Vec& A, // lower left coord
const Vec& B, // lower right coord
const Vec& C, // upper right coord
const Vec& D, // upper left coord
unsigned int divs = 2,
const Vec& At = Vec(0,0),
const Vec& Bt = Vec(1,0),
const Vec& Ct = Vec(1,1),
const Vec& Dt = Vec(0,1)
)
{
// base case
if( divs == 0 )
{
glTex2d( At );
glVec2d( A );
glTex2d( Bt );
glVec2d( B );
glTex2d( Ct );
glVec2d( C );
glTex2d( Dt );
glVec2d( D );
return;
}
Vec AB = (A+B) * 0.5;
Vec BC = (B+C) * 0.5;
Vec CD = (C+D) * 0.5;
Vec AD = (A+D) * 0.5;
Vec ABCD = (AB+CD) * 0.5;
Vec ABt = (At+Bt) * 0.5;
Vec BCt = (Bt+Ct) * 0.5;
Vec CDt = (Ct+Dt) * 0.5;
Vec ADt = (At+Dt) * 0.5;
Vec ABCDt = (ABt+CDt) * 0.5;
// subdivided point layout
// D CD C
//
// AD ABCD BC
//
// A AB B
// subdivide
glQuad2d( A, AB, ABCD, AD, divs - 1, At, ABt, ABCDt, ADt );
glQuad2d( AB, B, BC, ABCD, divs - 1, ABt, Bt, BCt, ABCDt );
glQuad2d( ABCD, BC, C, CD, divs - 1, ABCDt, BCt, Ct, CDt );
glQuad2d( AD, ABCD, CD, D, divs - 1, ADt, ABCDt, CDt, Dt );
}
It's currently recursive but you could always add an explicit stack for some for-loop action.