I have a Vector3d containing a list of vertices and I'm trying to apply a rotation on it, but can't find a way to do so:
rotationArbitraryAxis(const Vector3d axis, const double angle) {
Vector3d normAxis = axis.normalized();
double radians = angle * M_PI / 180.0f;
AngleAxisd aa(radians, normAxis);
Affine3d fTransform = Translation3d(normAxis) * aa * Translation3d(-normAxis);
const double* data = reinterpret_cast<const double*>(m_mesh.points().data());
Vector3d verts(m_mesh.number_of_vertices());
std::copy(data, data + m_mesh.number_of_vertices() * 3, verts.data());
verts *= ???
}
Any ideas?
As i said in my comment, convert the Affine3d to a matrix, and use that to transform the vertices.
const double* data = reinterpret_cast<const double*>(m_mesh.points().data());
// I'm assuming you want this as an array of vector3's ???
std::vector<Vector3d> verts(m_mesh.number_of_vertices());
std::copy(data, data + m_mesh.number_of_vertices() * 3, verts.data());
// convert the transform to a matrix
auto matrix = fTransform.matrix();
for(auto& vert : verts)
{
// now multiply
vert = matrix * vert;
}
Related
I'm trying to get the angles between two vectors (My Camera Position and Enemy Position) to create an autoaim/aimbot.
The game is Unity based, it uses the left handed coordinate system. X Y Z is right, up, forward.
The game also uses degrees.
Here is the pseudocode I am trying but its failing to give me the proper pitch/yaw.
diff = camera_position - enemy_position
hypotenuse = sqrt(diff.x*diff.x + diff.y*diff.y)
angle.x = asinf(diff.z / hypotenuse) * (180 / PI);
angle.y = atan2(diff.y / diff.x) * (180 / PI);
angle.z = 0.0f;
Can someone help me with this? I am terrible at math.
I'm trying to get the angles between two vectors (My Camera Position
and Enemy Position)
In Unity:
Use the Angle function from Vector3 structure.
float angle = Vector3.Angle(camera_position, enemy_position);
Or Individual angles:
float angleX = Vector3.Angle(new Vector3(camera_position.x, 0, 0), new Vector3(enemy_position.x, 0, 0));
float angleY = Vector3.Angle(new Vector3(0, camera_position.y, 0), new Vector3(0, enemy_position.y, 0));
float angleZ = Vector3.Angle(new Vector3(0, 0, camera_position.z), new Vector3(0, 0, enemy_position.z));
EDIT:
I'm not using the Unity engine. This is a separate module I am
creating to rig my own autoaim. I'm trying to do get the proper math
itself.
In C++:
The code is explained in the Angle function below which is the last function
#include <iostream>
#include <numeric> //for inner_product
#include <vector> //For vector
#include <math.h> //For sqrt, acos and M_PI
float Dot(std::vector<float> lhs, std::vector<float> rhs);
float magnitude(std::vector<float> vec3);
float Angle(std::vector<float> from, std::vector<float> to);
std::vector<float> normalise();
int main()
{
std::vector<float> from{3, 1, -2};
std::vector<float> to{5, -3, -7 };
float angle = Angle(from,to);
std::cout<<"Angle: "<<angle<<std::endl;
return 0;
}
//Find Dot/ Scalar product
float Dot(std::vector<float> lhs, std::vector<float> rhs){
return std::inner_product(lhs.begin(), lhs.end(), rhs.begin(), 0);
}
//Find the magnitude of the Vector
float magnitude(std::vector<float> vec3)//<! Vector magnitude
{
return sqrt((vec3[0] * vec3[0]) + (vec3[1] * vec3[1]) + (vec3[2] * vec3[2]));
}
//Normalize Vector. Not needed here
std::vector<float> normalise(std::vector<float> vect)
{
std::vector<float> temp{0, 0, 0};
float length = magnitude(vect);
temp[0] = vect[0]/length;
temp[1] = vect[1]/length;
temp[2] = vect[2]/length;
return temp;
}
float Angle(std::vector<float> from, std::vector<float> to){
//Find the scalar/dot product of the provided 2 Vectors
float dotProduct = Dot(from, to);
//Find the product of both magnitudes of the vectors then divide dot from it
dotProduct = dotProduct / (magnitude(from) * magnitude(to));
//Get the arc cosin of the angle, you now have your angle in radians
float arcAcos = acos(dotProduct);
//Convert to degrees by Multiplying the arc cosin by 180/M_PI
float angle = arcAcos * 180 / M_PI;
return angle;
}
To calculate the angle between two 3d coordinates, in degrees you can use this CalcAngle Function:
#include <algorithm>
#define PI 3.1415927f
struct vec3
{
float x, y, z;
}
vec3 Subtract(vec3 src, vec3 dst)
{
vec3 diff;
diff.x = src.x - dst.x;
diff.y = src.y - dst.y;
diff.z = src.z - dst.z;
return diff;
}
float Magnitude(vec3 vec)
{
return sqrtf(vec.x*vec.x + vec.y*vec.y + vec.z*vec.z);
}
float Distance(vec3 src, vec3 dst)
{
vec3 diff = Subtract(src, dst);
return Magnitude(diff);
}
vec3 CalcAngle(vec3 src, vec3 dst)
{
vec3 angle;
angle.x = -atan2f(dst.x - src.x, dst.y - src.y) / PI * 180.0f + 180.0f;
angle.y = asinf((dst.z - src.z) / Distance(src, dst)) * 180.0f / PI;
angle.z = 0.0f;
return angle;
}
Complications:
Not all games use the same technique for angles and positions. Min and Max values for x, y and z angles can be different in every game. The basic idea is the same in all games, they just require minor modification to match each game. For example, in the game the code was written for, the X value has to be made negative at the end for it to work.
Another complication is X, Y and Z don't always represent the same variables in both coordinates and angle vec3s.
How i can determine the CGAL affine transformation (Aff_transformation_3) that transform one plane (plane1) into a parallel plane to another (plane2)?
Suppose that i have two object planes:
Plane_3 pl1;
Plane_3 pl2;
and they are not parallels, how determine this kind of affine transformation?
Aff_transformation_3 t3 = ??? (pl1, pl2);
I consulted this question and your answer: CGAL: Transformation Matrix for Rotation given two lines/vectors/directions, but i don't know how it may helpme. I have two planes, but in 3d dimensions.
Thanks.
I don't know how a 2d affine transformation (Aff_transformation_2) may helpme to apply a 3d affine transformation (Aff_transformation_3).
However, i found the solution to my question. This is may bit code that i hope to help someone.
typedef CGAL::Cartesian<double> KC;
typedef KC::Line_3 Line3;
typedef KC::Vector_3 Vector3;
typedef KC::Plane_3 Plane3;
typedef CGAL::Aff_transformation_3<KC> Transform3;
// forwards
struct axis_angle;
typedef boost::shared_ptr<axis_angle> RAxisAngle;
struct axis_angle
{
axis_angle()
{
angle = 0;
axis = Vector3(0.0, 0.0, 0.0);
}
double angle;
Vector3 axis;
};
Vector3 normalize(const Vector3 &v)
{
ldouble len = ::sqrt(v.squared_length());
if (len == 0.0)
return v;
return v / len;
}
// return the angle and axis from two planes that there are not parallels
RAxisAngle axis_angle_from_planes(const Plane3 &pln1, const Plane3 &pln2)
{
RAxisAngle result = RAxisAngle(new axis_angle());
Vector3 norm1 = pln1.orthogonal_vector();
Vector3 norm2 = pln2.orthogonal_vector();
double dot_r = norm1 * norm2;
double len_r = ::sqrt(norm1.squared_length() * norm2.squared_length());
if (len_r)
result->angle = ::acos(dot_r / len_r);
else
result->angle = 0.0;
Line3 l1;
CGAL::Object obj_cgal = CGAL::intersection(pln1, pln2);
if (CGAL::assign(l1, obj_cgal))
{
result->axis = normalize(l1.to_vector());
}
else
{
// when planes are parallels, then use some basic axis
result->axis = Vector3(1.0, 0.0, 0.0);
}
return result;
}
// return a CGAL affine transformation that is builded from a 3x3 matrix
// this transformation is for rotate an object from axis and angle
// http://en.wikipedia.org/wiki/Transformation_matrix
// http://en.wikipedia.org/wiki/Rotation_matrix
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm
Transform3 axis_angle_to_matrix(const RAxisAngle &aa)
{
double tmp1, tmp2;
double c = ::cos(aa->angle);
double s = ::sin(aa->angle);
double t = 1.0 - c;
double m00 = c + aa->axis.x() * aa->axis.x() * t;
double m11 = c + aa->axis.y() * aa->axis.y() * t;
double m22 = c + aa->axis.z() * aa->axis.z() * t;
tmp1 = aa->axis.x() * aa->axis.y() * t;
tmp2 = aa->axis.z() * s;
double m10 = tmp1 + tmp2;
double m01 = tmp1 - tmp2;
tmp1 = aa->axis.x() * aa->axis.z() * t;
tmp2 = aa->axis.y() * s;
double m20 = tmp1 - tmp2;
double m02 = tmp1 + tmp2;
tmp1 = aa->axis.y() * aa->axis.z() * t;
tmp2 = aa->axis.x() * s;
double m21 = tmp1 + tmp2;
double m12 = tmp1 - tmp2;
return Transform3(m00, m01, m02, m10, m11, m12, m20, m21, m22);
}
Then, i can use there as this:
RAxisAngle aa = axis_angle_from_planes(plane1, plane2);
Transform3 t3 = axis_angle_to_matrix(aa);
Plane2 new_transform_plane = plane1.transform(t3);
or maybe a point of this plane:
Point3 new_transform_point = point_of_plane1.transform(t3);
Thanks for giveme the posibility to post my little solution.
I'm creating a 3D application using GLUT in C++.
Now, I want to implement a method similar to this:
Vector3* MyClass::get3DObjectfromMouse(int mouseX, int mouseY);
How can I implement this method?
As it was commented by Andon M. Coleman, one way you can achieve this is by doing a ray/object intersection test, with unprojected screen coordinates. This technique is commonly known as picking.
A pseudo-C++ code for picking:
Assume we have a 3D object type/class:
class Object3D { ... };
A 3D picking function would return a list of all objects that are intersected by a line going from the given 2D point in the near plane to the same point in the far plane.
struct LineSegment
{
Vector3 start;
Vector3 end;
};
Object3D[] Pick(float x, float y)
{
LineSegment lineSeg;
Object3D[] intersectedObjs;
// Do both un-projections for z-near (0) and z-far (1).
// This produces a line segment going from z-near to far.
UnProject(x, y, /* z = */ 0.0, modelViewMatrix, projectionMatrix, viewport, lineSeg.start);
UnProject(x, y, /* z = */ 1.0, modelViewMatrix, projectionMatrix, viewport, lineSeg.end);
// Iterate all object in the scene or in the current view:
for (Object3D obj : scene)
{
if (TestLineIntersection(obj, lineSeg))
{
// This object is crossed by the picking line.
intersectedObjs.Add(obj);
}
}
// Optionally you might want sort them from distance
// to the camera/viewer before returning the intersections.
return intersectedObjs;
}
And the UnProject() function would look like this:
bool UnProject(float winX, float winY, float winZ,
const Matrix4 & modelView, const Matrix4 & projection,
const ScreenRect viewport, Vector3 & worldCoordinates)
{
// Compute (projection x modelView) ^ -1:
const Matrix4 m = inverse(projection * modelView);
// Need to invert Y since screen Y-origin point down,
// while 3D Y-origin points up (this is an OpenGL only requirement):
winY = viewport.Height() - winY;
// Transformation of normalized coordinates between -1 and 1:
Vector4 in;
in[0] = (winX - viewport.X()) / viewport.Width() * 2.0 - 1.0;
in[1] = (winY - viewport.Y()) / viewport.Height() * 2.0 - 1.0;
in[2] = 2.0 * winZ - 1.0;
in[3] = 1.0;
// To world coordinates:
Vector4 out(m * in);
if (out[3] == 0.0) // Avoid a division by zero
{
worldCoordinates = Vector3Zero;
return false;
}
out[3] = 1.0 / out[3];
worldCoordinates[0] = out[0] * out[3];
worldCoordinates[1] = out[1] * out[3];
worldCoordinates[2] = out[2] * out[3];
return true;
}
To clarify, TestLineIntersection() does a line vs AABB intersection test. The bounding box should be transformed to world-space, since it is usually expressed as a set of points in local model-space.
bool TestLineIntersection(const Object3D & obj, const LineSegment & lineSeg)
{
AABB aabb = obj.GetAABB();
aabb.TransformBy(obj.modelMatrix);
return aabb.LineIntersection(lineSeg.start, lineSeg.end);
}
// AABB.cpp:
bool AABB::LineIntersection(const Vector3 & start, const Vector3 & end) const
{
const Vector3 center = (mins + maxs) * 0.5;
const Vector3 extents = maxs - center;
const Vector3 lineDir = 0.5 * (end - start);
const Vector3 lineCenter = start + lineDir;
const Vector3 dir = lineCenter - center;
const float ld0 = Mathf::Abs(lineDir[0]);
if (Mathf::Abs(dir[0]) > (extents[0] + ld0))
{
return false;
}
const float ld1 = Mathf::Abs(lineDir[1]);
if (Mathf::Abs(dir[1]) > (extents[1] + ld1))
{
return false;
}
const float ld2 = Mathf::Abs(lineDir[2]);
if (Mathf::Abs(dir[2]) > (extents[2] + ld2))
{
return false;
}
const Vector3 vCross = cross(lineDir, dir);
if (Mathf::Abs(vCross[0]) > (extents[1] * ld2 + extents[2] * ld1))
{
return false;
}
if (Mathf::Abs(vCross[1]) > (extents[0] * ld2 + extents[2] * ld0))
{
return false;
}
if (Mathf::Abs(vCross[2]) > (extents[0] * ld1 + extents[1] * ld0))
{
return false;
}
return true;
}
We have 3(three) xyz points that define a circle in 3D space, this circle needs to be converted into a polyline(for further rendering). I'm looking for a ready C or C++ function or free library that can do the job.
Don't understand why this was closed. And I can't even answer my own question there. Shame on you guys. But you will not stop the knowledge spreading!
There's a much simpler solution to find the circle parameters in real 3D, just take a look at the "barycentric coordinates" section in http://en.wikipedia.org/wiki/Circumscribed_circle .
You can extract the following optimized code from that:
// triangle "edges"
const Vector3d t = p2-p1;
const Vector3d u = p3-p1;
const Vector3d v = p3-p2;
// triangle normal
const Vector3d w = t.crossProduct(u);
const double wsl = w.getSqrLength();
if (wsl<10e-14) return false; // area of the triangle is too small (you may additionally check the points for colinearity if you are paranoid)
// helpers
const double iwsl2 = 1.0 / (2.0*wsl);
const double tt = t*t;
const double uu = u*u;
// result circle
Vector3d circCenter = p1 + (u*tt*(u*v) - t*uu*(t*v)) * iwsl2;
double circRadius = sqrt(tt * uu * (v*v) * iwsl2*0.5);
Vector3d circAxis = w / sqrt(wsl);
You can then calculate the points on the circle in real 3D too and e.g. draw them using GL_LINE_STRIP in OpenGL. This should be much faster than using the 2D sin/cos approach.
// find orthogonal vector to the circle axis
const Vector3d an = circAxis.getNormalized();
const Vector3d ao = Vector3d(4.0+an[0], 4.0+an[0]+an[1], 4.0+an[0]+an[1]+an[2]).crossProduct(an).getNormalized();
// 4x4 rotation matrix around the circle axis
const int steps = 360; // maybe adjust according to circle size on screen
Matrix4d R = makeRotMatrix4d(circCenter, circAxis, 2.0*M_PI/double(steps));
// one point on the circle
Vector3d cp = circCenter + ao*circRadius;
// rotate point on the circle
for (int i=0; i<steps; ++i)
{
circlePoints.push_back(cp);
cp = transformPoint(cp, R); // apply the matrix
}
For the creation of the transformation matrix (i.e. makeRotMatrix4d()) see http://paulbourke.net/geometry/rotate/ for example.
Please note that I did not test if the above code really compiles, but it should give you enough hints.
There is a nice article and a code sample on how to build a circle by 3 points in 2D, XY plane.
http://paulbourke.net/geometry/circlesphere/
http://paulbourke.net/geometry/circlesphere/Circle.cpp
To build a 3D circle we'll have to:
rotate our 3 points into XY plane
Calculate circle center
build a circle in XY plane using the code in the article
rotate it back into it's original plane
For rotations it is best to use quaternions.
To find a correct quaternion I looked at Ogre3d source code:
void Quaternion::FromAngleAxis (const Radian& rfAngle, const Vector3& rkAxis)
There is one more useful function there:
Quaternion getRotationTo(const Vector3& dest, const Vector3& fallbackAxis = Vector3::ZERO) const
But I didn't use it.
For quaterions and vectors I used our own classes. Here is the full source code of the function that does the job:
bool IsPerpendicular(Point3d *pt1, Point3d *pt2, Point3d *pt3);
double CalcCircleCenter(Point3d *pt1, Point3d *pt2, Point3d *pt3, Point3d *center);
void FindCircleCenter(const Point3d *V1, const Point3d *V2, const Point3d *V3, Point3d *center)
{
Point3d *pt1=new Point3d(*V1);
Point3d *pt2=new Point3d(*V2);
Point3d *pt3=new Point3d(*V3);
if (!IsPerpendicular(pt1, pt2, pt3) ) CalcCircleCenter(pt1, pt2, pt3, center);
else if (!IsPerpendicular(pt1, pt3, pt2) ) CalcCircleCenter(pt1, pt3, pt2, center);
else if (!IsPerpendicular(pt2, pt1, pt3) ) CalcCircleCenter(pt2, pt1, pt3, center);
else if (!IsPerpendicular(pt2, pt3, pt1) ) CalcCircleCenter(pt2, pt3, pt1, center);
else if (!IsPerpendicular(pt3, pt2, pt1) ) CalcCircleCenter(pt3, pt2, pt1, center);
else if (!IsPerpendicular(pt3, pt1, pt2) ) CalcCircleCenter(pt3, pt1, pt2, center);
else {
delete pt1;
delete pt2;
delete pt3;
return;
}
delete pt1;
delete pt2;
delete pt3;
}
bool IsPerpendicular(Point3d *pt1, Point3d *pt2, Point3d *pt3)
// Check the given point are perpendicular to x or y axis
{
double yDelta_a= pt2->y - pt1->y;
double xDelta_a= pt2->x - pt1->x;
double yDelta_b= pt3->y - pt2->y;
double xDelta_b= pt3->x - pt2->x;
// checking whether the line of the two pts are vertical
if (fabs(xDelta_a) <= 0.000000001 && fabs(yDelta_b) <= 0.000000001){
return false;
}
if (fabs(yDelta_a) <= 0.0000001){
return true;
}
else if (fabs(yDelta_b) <= 0.0000001){
return true;
}
else if (fabs(xDelta_a)<= 0.000000001){
return true;
}
else if (fabs(xDelta_b)<= 0.000000001){
return true;
}
else
return false ;
}
double CalcCircleCenter(Point3d *pt1, Point3d *pt2, Point3d *pt3, Point3d *center)
{
double yDelta_a = pt2->y - pt1->y;
double xDelta_a = pt2->x - pt1->x;
double yDelta_b = pt3->y - pt2->y;
double xDelta_b = pt3->x - pt2->x;
if (fabs(xDelta_a) <= 0.000000001 && fabs(yDelta_b) <= 0.000000001){
center->x= 0.5*(pt2->x + pt3->x);
center->y= 0.5*(pt1->y + pt2->y);
center->z= pt1->z;
return 1;
}
// IsPerpendicular() assure that xDelta(s) are not zero
double aSlope=yDelta_a/xDelta_a; //
double bSlope=yDelta_b/xDelta_b;
if (fabs(aSlope-bSlope) <= 0.000000001){ // checking whether the given points are colinear.
return -1;
}
// calc center
center->x= (aSlope*bSlope*(pt1->y - pt3->y) + bSlope*(pt1->x + pt2 ->x)
- aSlope*(pt2->x+pt3->x) )/(2* (bSlope-aSlope) );
center->y = -1*(center->x - (pt1->x+pt2->x)/2)/aSlope + (pt1->y+pt2->y)/2;
return 1;
}
//! Builds a circle in 3D space by 3 points on it and an optional center
void buildCircleBy3Pt(const float *pt1,
const float *pt2,
const float *pt3,
const float *c, // center, can be NULL
std::vector<float> *circle)
{
/* Get the normal vector to the triangle formed by 3 points
Calc a rotation quaternion from that normal to the 0,0,1 axis
Rotate 3 points using quaternion. Points will be in XY plane
Build a circle by 3 points on XY plane
Rotate a circle back into original plane using quaternion
*/
Point3d p1(pt1[0], pt1[1], pt1[2]);
Point3d p2(pt2[0], pt2[1], pt2[2]);
Point3d p3(pt3[0], pt3[1], pt3[2]);
Point3d center;
if (c)
{
center.set(c[0], c[1], c[2]);
}
const Vector3d p2top1 = p1 - p2;
const Vector3d p2top3 = p3 - p2;
const Vector3d circle_normal = p2top1.crossProduct(p2top3).normalize();
const Vector3d xy_normal(0, 0, 1);
Quaternion rot_quat;
// building rotation quaternion
{
// Rotation axis around which we will rotate our circle into XY plane
Vector3d rot_axis = xy_normal.crossProduct(circle_normal).normalize();
const double rot_angle = xy_normal.angleTo(circle_normal); // radians
const double w = cos(rot_angle * 0.5);
rot_axis *= sin(rot_angle * 0.5);
rot_quat.set(w, rot_axis.x, rot_axis.y, rot_axis.z);
}
Quaternion rot_back_quat;
// building backward rotation quaternion, same as prev. but -angle
{
const double rot_angle = -(xy_normal.angleTo(circle_normal)); // radians
const double w_back = cos(rot_angle * 0.5);
Vector3d rot_back_axis = xy_normal.crossProduct(circle_normal).normalize();
rot_back_axis *= sin(rot_angle * 0.5);
rot_back_quat.set(w_back, rot_back_axis.x, rot_back_axis.y, rot_back_axis.z);
}
rot_quat.rotate(p1);
rot_quat.rotate(p2);
rot_quat.rotate(p3);
rot_quat.rotate(center);
if (!c)
{
// calculate 2D center
FindCircleCenter(&p1, &p2, &p3, ¢er);
}
// calc radius
const double radius = center.distanceTo(p1);
const float DEG2RAD = 3.14159f / 180.0f;
// build circle
for (int i = 0; i < 360; ++i)
{
float degInRad = i * DEG2RAD;
Point3d pt(cos(degInRad) * radius + center.x, sin(degInRad) * radius + center.y, 0);
// rotate the point back into original plane
rot_back_quat.rotate(pt);
circle->push_back(pt.x);
circle->push_back(pt.y);
circle->push_back(pt.z);
}
}
The following is the C#/Unity port of Mark's answer. It uses types and utility functions from Unity's scripting API.
// triangle "edges"
var t = p2 - p1;
var u = p3 - p1;
var v = p3 - p2;
// triangle normal
var w = Vector3.Cross(t, u);
var wsl = Vector3.Dot(w, w);
// TODO: if (wsl<10e-14) return false; // area of the triangle is too small (you may additionally check the points for colinearity if you are paranoid)
// helpers
var iwsl2 = 1f / (2f * wsl);
var tt = Vector3.Dot(t, t);
var uu = Vector3.Dot(u, u);
// result circle
Vector3 circCenter = p1 + (u * tt * (Vector3.Dot(u, v)) - t * uu * (Vector3.Dot(t, v))) * iwsl2;
var circRadius = Mathf.Sqrt(tt * uu * (Vector3.Dot(v, v)) * iwsl2 * 0.5f);
Vector3 circAxis = w / Mathf.Sqrt(wsl);
Using Unity's Gizmos, the circle can be drawn as follows (using 30 line segments to approximate it in this case):
// Draw the circle:
Gizmos.color = Color.white;
for (int i = 0; i < 30; ++i)
{
Gizmos.DrawLine(
circCenter + Quaternion.AngleAxis(360f / 30f * i , circAxis) * (p1 - circCenter),
circCenter + Quaternion.AngleAxis(360f / 30f * (i + 1), circAxis) * (p1 - circCenter)
);
}
The result looks like follows for vertex positions var p1 = new Vector3(0f, 1.44f, 0f); var p2 = new Vector3(0f, 0.73f, 0.65f); var p3 = new Vector3(0f, -1.04f, 0f);:
I'm working on a program with IK and have run into what I had at first thought was a trivial problem but have since had trouble solving it.
Background:
Everything is in 3d space.
I'm using 3d Vectors and Quaternions to represent transforms.
I have a limb which we will call V1.
I want to rotate it onto V2.
I was getting the angle between V1 and V2.
Then the axis for rotation by V1 cross V2.
Then making a Quaternion from the axis and angle.
I then take the limbs current Orientation and multiply it by the axis angle quaternion.
This I believe is my desired local space for the limb.
This limb is attached to a series of other links. To get the world space I traverse up to the root combining the parents local space with the child's local space until I reach the root.
This seems to work grand if the vector that I am rotating to is contained within the X and Y plane or if the body which the limb is attached to hasn't been modified. If anything has been modified, for example rotating the root node, then on the first iteration the vector will rotate very close to the desired vector. After that point though it will begin to spin all over the place and never reach the goal.
I've gone through all the math line by line and it appears to all be correct. I'm not sure if there is something that I do not know about or am simply over looking. Is my logical sound? Or am I unaware of something? Any help is greatly appreciated!
Quaternion::Quaternion(const Vector& axis, const float angle)
{
float sin_half_angle = sinf( angle / 2 );
v.set_x( axis.get_x() * sin_half_angle );
v.set_y( axis.get_y() * sin_half_angle );
v.set_z( axis.get_z() * sin_half_angle );
w = cosf( angle / 2 );
}
Quaternion Quaternion::operator* (const Quaternion& quat) const
{
Quaternion result;
Vector v1( this->v );
Vector v2( quat.v );
float s1 = this->w;
float s2 = quat.w;
result.w = s1 * s2 - v1.Dot(v2);
result.v = v2 * s1 + v1 * s2 + v1.Cross(v2);
result.Normalize();
return result;
}
Vector Quaternion::operator* (const Vector& vec) const
{
Quaternion quat_vec(vec.get_x(), vec.get_y(), vec.get_z(), 0.0f);
Quaternion rotation( *this );
Quaternion rotated_vec = rotation * ( quat_vec * rotation.Conjugate() );
return rotated_vec.v;
}
Quaternion Quaternion::Conjugate()
{
Quaternion result( *this );
result.v = result.v * -1.0f;
return result;
}
Transform Transform::operator*(const Transform tran)
{
return Transform( mOrient * transform.getOrient(), mTrans + ( mOrient * tran.getTrans());
}
Transform Joint::GetWorldSpace()
{
Transform world = local_space;
Joint* par = GetParent();
while ( par )
{
world = par->GetLocalSpace() * world;
par = par->GetParent();
}
return world;
}
void RotLimb()
{
Vector end_effector_worldspace_pos = end_effector->GetWorldSpace().get_Translation();
Vector parent_worldspace_pos = parent->GetWorldSpace().get_Translation();
Vector parent_To_end_effector = ( end_effector_worldspace_pos - parent_worldspace_pos ).Normalize();
Vector parent_To_goal = ( goal_pos - parent_worldspace_pos ).Normalize();
float dot = parent_To_end_effector.Dot( parent_To_goal );
Vector rot_axis(0.0f,0.0f,1.0f);
float angle = 0.0f;
if (1.0f - fabs(dot) > EPSILON)
{
//angle = parent_To_end_effector.Angle( parent_To_goal );
rot_axis = parent_To_end_effector.Cross( parent_To_goal ).Normalize();
parent->RotateJoint( rot_axis, acos(dot) );
}
}
void Joint::Rotate( const Vector& axis, const float rotation )
{
mLocalSpace = mlocalSpace * Quaternion( axis, rotation );
}
You are correct when you write in a comment that the axis should be computed in the local coordinate frame of the joint:
I'm wondering if this issue is occuring because I'm doing the calculations to get the axis and angle in the world space for the joint, but then applying it to the local space.
The rotation of the axis from the world frame to the joint frame will look something like this:
rot_axis = parent->GetWorldSpace().Inverse().get_Rotation() * rot_axis
There can be other issues to debug, but it's the only logical error I can see in the code that you have posted.