3D matrices: absolute to relative and vice-versa transformations - opengl

I have an OpenGL C# project, that I would like to give functionality like Unity3D game engine.
Introduction:
I have Transform class that provides transformations matrix to shader. Each transform can have parent transform. Code that calculates final transformations matrix looks like this:
public Vector3 LocalPosition { get; set; }
public Quaternion LocalRotation { get; set; }
public Vector3 LocalScale { get; set; }
public Matrix GetModelMatrix() {
Matrix result;
if(HasParent)
result = Parent.GetModelMatrix();
else
result = Matrix.CreateIdentity();
ApplyLocalTransformations(result);
return result;
}
private void ApplyLocalTransform(Matrix matrix)
{
matrix.Translate(LocalPosition);
matrix.Rotate(LocalRotation);
matrix.Scale(LocalScale);
}
As you see LocalPosition, LocalScale and LocalRotation are transformations RELATIVE to parent.
This code works fine.
Problem:
I want to add 3 more properties (hello Unity3D):
public Vector3 AbsolutePosition { get; set; }
public Quaternion AbsoluteRotation { get; set; }
public Vector3 AbsoluteScale { get; set; }
I want to have ability to get and set absolute transformations to child transforms. While setting Absolute values Local should be updated consistently and vice versa.
Example: We have parent at position (1, 1, 1) and child with LocalPosition = (0, 0, 0), having this information we can calculate child's AbsolutePosition = (1, 1, 1).
Now we set child's AbsolutePosition = (0, 0, 0). It's LocalPosition will now be = (-1, -1, -1).
It's a very simple example, in real scenario we have to consider parent's scale and rotation to calculate Position.
How to calculate Absolute and Local Position i have an idea:
I can take last column from transformations matrix and it will be my AbsolutePosition. To get LocalPosition i can subtract from AbsolutePosition last column of parent transformations matrix. But mathematics behind Rotation and Scale still unclear for me.
Question:
Can you help me with algorithm that will calculate Local and Absolute Position, Rotation and Scale?
P.S.: considering performance would be great.

I have dealt with this exact problem. There is more than one way to solve it and so I will just give you the solution that I came up with. In short, I store the position, rotation and scale in both local and world coordinates. I then calculate deltas so that I can apply changes made in one coordinate space to the other.
Finally, I use events to broadcast the deltas to all descended game objects. Events are not strictly necessary. You could just recursively call some functions on the transform components of descended game objects in order to apply the deltas down the game object tree.
It's probably best to give an example at this point, so take a look at this setter method for the transform's local position (which I have lifted from a very small game that I worked on):
void Transform::localPosition(const Vector3& localPosition)
{
const Vector3 delta = localPosition - m_localPosition;
m_localPosition = localPosition; // Set local position.
m_position += delta; // Update world position.
// Now broadcast the delta to all descended game objects.
}
So that was trivial. The setter for the world position is similar:
void Transform::position(const Vector3& position)
{
const Vector3 delta = position - m_position;
m_position = position; // Set world position.
m_localPosition += delta; // Update local position.
// Now broadcast the delta to all descended game objects.
}
The principle is the same for rotation:
void Transform::localRotation(const Quaternion& localRotation)
{
const Quaternion delta = m_localRotation.inverse() * localRotation;
m_localRotation = localRotation; // Set the local orientation.
m_rotation = delta * m_rotation; // Update the world orientation.
// Now broadcast the delta to all descended game objects.
}
void Transform::rotation(const Quaternion& rotation)
{
const Quaternion delta = m_rotation.inverse() * rotation;
m_rotation = rotation; // Set the world orientation.
m_localRotation = delta * m_localRotation; // Update the local orientation.
// Now broadcast the delta to all descended game objects.
}
And finally scale:
void Transform::localScale(const Vector3& scale)
{
const Vector3 delta = scale - m_localScale;
m_localScale = scale; // Set the local scale.
m_scale += delta; // Update the world scale.
// Now broadcast the delta to all descended game objects.
}
void Transform::scale(const Vector3& scale)
{
const Vector3 delta = scale - m_scale;
m_scale = scale; // Set the world scale.
m_localScale += delta; // Update the local scale.
// Now broadcast the delta to all descended game objects.
}
I'm not sure how you could improve on this from a performance perspective. Computing and applying deltas is relatively cheap (certainly much cheaper than decomposing transformation matrices).
Finally, since you are attempting to emulate Unity, you might want to take a look at my small c++ mathematics library, which is modeled on Unity's maths classes.
Example calculations
So I left out quite a few details in my original answer which seems to have caused some confusion. I provide below a detailed example that follows the concept of using deltas (as described above) in response to Xorza's comment.
I have a game object that has one child. I will refer to these game objects as parent and child respectively. They both have default scale (1, 1, 1) and are positioned at the origin (0, 0, 0).
Note that Unity's Transform class does not allow writing to the lossyScale (world scale) property. So, following the behavior provided by Unity, I will deal with modifications to the parent transform's localScale property.
Firstly, I call parent.transform.setLocalScale(0.1, 0.1, 0.1).
The setLocalScale function writes the new value to the localScale field and then calculates the scaling delta as follows:
scalingDelta = newLocalScale / oldLocalScale
= (0.1, 0.1, 0.1) / (1, 1, 1)
= (0.1, 0.1, 0.1)
We use this scaling delta to update the transform's world scale property.
scale = scalingDelta * scale;
Now, because changes to the parent's transform properties (local or world) affect the child transform's world properties, I need to update the child transform's world properties. In particular, I need to update the child transform's scale and position properties (rotation is not affected in this particular operation). We can do this as follows:
child.transform.scale = scalingDelta * child.transform.scale
= (0.1, 0.1, 0.1) * (1, 1, 1)
= (0.1, 0.1, 0.1)
child.transform.position = parent.transform.position + scalingDelta * child.transform.localPosition
= (child.transform.position - child.transform.localPosition) + scalingDelta * child.transform.localPosition
= ((0, 0, 0) - (0, 0, 0)) + (0.1, 0.1, 0.1) * (0, 0, 0)
= (0, 0, 0)
Note that accessing the parent transform's position is difficult if you use events to pass the deltas down the game object tree. However, since child.transform.position = parent.transforn.position + child.transform.localPosition, we can compute the parent transform's world position from the child transform's world position and local position.
Also, importantly, note that the child transform's local properties are not changed.
Secondly, I call child.transform.setPosition(1, 1, 1).
The setPosition function writes the new value to position and then calculates the translation delta as follows:
translationDelta = newPosition - oldPosition
= (1, 1, 1) - (0, 0, 0)
= (1, 1, 1)
Finally, the setPosition function updates the transform's localPosition using the computed delta. However, note that the computed translation delta is in world space coordinates. So we need to do a little work to convert it into local space coordinates before updating localPosition. In particular, we need to take into account the parent transform's world scale.
localPosition = localPosition + translationDelta / parent.transform.scale
= localPosition + translationDelta / (scale / localScale)
= localPosition + translationDelta * (localScale / scale)
= (0, 0, 0) + (1, 1, 1) * ((1, 1, 1,) / (0.1, 0.1, 0.1))
= (10, 10, 10)
Again, it is not necessary to look up the parent transform's world scale. This can be calculated from the child transform's world scale and local scale.
In this example I dealt with changes to the parent transform's scale. The same principles apply for changes to the parent's position and rotation, although the calculations will be different.

Related

Bullet physics to Physx Get Orientation

For a game that I am developing I moved from Bullet physics to NVidia Physx. The problem that I have is the following. I used to have a translation from bullet's rigid body orientation quaternion to Front , Right and Up vector for each object on the screen. This was working fine but after I moved to Physx I noticed that there is only one quaternion in the transform of the object (probably representing rotation) and no orientation quaternion. Here is the code that I was using in Bullet to get the 3 vectors translated to Physx :
physx::PxQuat ori = mBody->getGlobalPose().q;
auto Orientation = glm::quat(ori.x, ori.y, ori.z, ori.w);
glm::quat qF = Orientation * glm::quat(0, 0, 0, 1) * glm::conjugate(Orientation);
glm::quat qUp = Orientation * glm::quat(0, 0, 1, 0) * glm::conjugate(Orientation);
front = { qF.x, qF.y, qF.z };
up = { qUp.x, qUp.y, qUp.z };
right = glm::normalize(glm::cross(front, up));
This apparently doesn't work for Physx. Is there another way to retrieve the 3 vectors from the rigid body ?

How to find Relative Offset of a point inside a non axis aligned box (box that is arbitrarily rotated)

I'm trying to solve an problem where I cannot find the Relative Offset of a Point inside a Box that exists inside of a space that can be arbitrarily rotated and translated.
I know the WorldSpace Location of the Box (and its 4 Corners, the Coordinates on the Image are Relative) as well as its Rotation. These can be arbitrary (its actually a 3D Trigger Volume within a game, but we are only concerned with it in a 2D plane from top down).
Looking at it Aligned to an Axis the Red Point Relative position would be
0.25, 0.25
If the Box was to be Rotated arbitrarily I cannot seem to figure out how to maintain that given we sample the same Point (its World Location will have changed) its Relative Position doesnt change even though the World Rotation of the Box has.
For reference, the Red Point represents an Object that exists in the scene that the Box is encompassing.
bool UPGMapWidget::GetMapMarkerRelativePosition(UPGMapMarkerComponent* MapMarker, FVector2D& OutPosition)
{
bool bResult = false;
if (MapMarker)
{
const FVector MapMarkerLocation = MapMarker->GetOwner()->GetActorLocation();
float RelativeX = FMath::GetMappedRangeValueClamped(
-FVector2D(FMath::Min(GetMapVolume()->GetCornerTopLeftLocation().X, GetMapVolume()->GetCornerBottomRightLocation().X), FMath::Max(GetMapVolume()->GetCornerTopLeftLocation().X, GetMapVolume()->GetCornerBottomRightLocation().X)),
FVector2D(0.f, 1.f),
MapMarkerLocation.X
);
float RelativeY = FMath::GetMappedRangeValueClamped(
-FVector2D(FMath::Min(GetMapVolume()->GetCornerTopLeftLocation().Y, GetMapVolume()->GetCornerBottomRightLocation().Y), FMath::Max(GetMapVolume()->GetCornerTopLeftLocation().Y, GetMapVolume()->GetCornerBottomRightLocation().Y)),
FVector2D(0.f, 1.f),
MapMarkerLocation.Y
);
OutPosition.X = FMath::Abs(RelativeX);
OutPosition.Y = FMath::Abs(RelativeY);
bResult = true;
}
return bResult;
}
Currently, you can see with the above code that im only using the Top Left and Bottom Right corners of the Box to try and calculate the offset, I know this is not a sufficient solution as doing this does not allow for Rotation (Id need to use the other 2 corners as well) however I cannot for the life of me work out what I need to do to reach the solution.
FMath::GetMappedRangeValueClamped
This converts one range onto another. (20 - 50) becomes (0 - 1) for example.
Any assistance/advice on how to approach this problem would be much appreciated.
Thanks.
UPDATE
#Voo's comment helped me realize that the solution was much simpler than anticipated.
By knowing the Location of 3 of the Corners of the Box, I'm able to find the points on the 2 lines these 3 Locations create, then simply mapping those points into a 0-1 range gives the appropriate value regardless of how the Box is Translated.
bool UPGMapWidget::GetMapMarkerRelativePosition(UPGMapMarkerComponent* MapMarker, FVector2D& OutPosition)
{
bool bResult = false;
if (MapMarker && GetMapVolume())
{
const FVector MapMarkerLocation = MapMarker->GetOwner()->GetActorLocation();
const FVector TopLeftLocation = GetMapVolume()->GetCornerTopLeftLocation();
const FVector TopRightLocation = GetMapVolume()->GetCornerTopRightLocation();
const FVector BottomLeftLocation = GetMapVolume()->GetCornerBottomLeftLocation();
FVector XPlane = FMath::ClosestPointOnLine(TopLeftLocation, TopRightLocation, MapMarkerLocation);
FVector YPlane = FMath::ClosestPointOnLine(TopLeftLocation, BottomLeftLocation, MapMarkerLocation);
// Convert the X axis into a 0-1 range.
float RelativeX = FMath::GetMappedRangeValueUnclamped(
FVector2D(GetMapVolume()->GetCornerTopLeftLocation().X, GetMapVolume()->GetCornerTopRightLocation().X),
FVector2D(0.f, 1.f),
XPlane.X
);
// Convert the Y axis into a 0-1 range.
float RelativeY = FMath::GetMappedRangeValueUnclamped(
FVector2D(GetMapVolume()->GetCornerTopLeftLocation().Y, GetMapVolume()->GetCornerBottomLeftLocation().Y),
FVector2D(0.f, 1.f),
YPlane.Y
);
OutPosition.X = RelativeX;
OutPosition.Y = RelativeY;
bResult = true;
}
return bResult;
}
The above code is the amended code from the original question with the correct solution.
assume the origin is at (x0, y0), the other three are at (x_x_axis, y_x_axis), (x_y_axis, y_y_axis), (x1, y1), the object is at (x_obj, y_obj)
do these operations to all five points:
(1)translate all five points by (-x0, -y0), to make the origin moved to (0, 0) (after that (x_x_axis, y_x_axis) is moved to (x_x_axis - x0, y_x_axis - y0));
(2)rotate all five points around (0, 0) by -arctan((y_x_axis - y0)/(x_x_axis - x0)), to make the (x_x_axis - x0, y_x_axis - y0) moved to x_axis;
(3)assume the new coordinates are (0, 0), (x_x_axis', 0), (0, y_y_axis'), (x_x_axis', y_y_axis'), (x_obj', y_obj'), then the object's zero-one coordinate is (x_obj'/x_x_axis', y_obj'/y_y_axis');
rotate formula:(x_new, y_new)=(x_old * cos(theta) - y_old * sin(theta), x_old * sin(theta) + y_old * cos(theta))
Update:
Note:
If you use the distance method, you have to take care of the sign of the coordinate if the object might go out of the scene in the future;
If there will be other transformations on the scene in the future (like symmetry transformation if you have mirror magic in the game, or transvection transformation if you have shockwaves, heatwaves or gravitational waves in the game), then the distance method no longer applies and you still have to reverse all the transformations your scene has in order to get the object's coordinate.

How create a atom model in c++ with irrlicht

I am working on a project in which I show a model of a atom with protons, neutrons, and electrons. I want to show the neutrons and protons like a atom model would and so I need some points to put my points around. So I need an function like this
<list of points> getPoints(int numberOfProtonsAndNeutrons)
I have tried iterating over the latitude and longitude, but I couldn't get the intervals to work. That also is not how atom model are visually shown.
So I need an idea of how to go about creating points to put my neutrons and protons on.
Well, assuming you want some neutrons and protons in the center, with some electrons outside in various orbits (the Bohr model), then you need a few things. We can just place all the protons and neutrons on the surface of a sphere with small radius (r=1), and then place the electrons on a sphere with radius increasing depending on the orbital shell the electron is on.
Without doing all the work for you, you need will need an atom class.
It will have a member representing the orbital shell using an index: i=0 could be the innermost shell, with i>0 defining the outer shells with electrons.
You could then compute the radius of the sphere using the index as a member function. Something like
radius() { return i * ShellDist + 1; }
where ShellDist is how far apart the electron shells should be.
You can create a class for the general atom, given its shell and type.
class Atom {
public:
static const double ShellDist = 2; // remember to include a definition for this
Atom(int shell) : shell(shell) {}
virtual void radius() { return shell * ShellDist + 1; }
private:
int shell;
};
class Electron : public Atom {
public:
Electron(int shell) : Atom(shell) {}
};
class Neutron : public Atom {
public:
Neutron() : Atom(0) {}
};
I'm assuming you want all the points on the sphere to be on a plane coincident with the sphere, with the plane going through the origin. To simplify this, we will just choose the circle which goes along the equator of the sphere, and then rotate it using a rotation matrix R.
So the equator circle's equation if z is the vertical axis
Vector3d
circle(double t, double r) { return Vector3d(cos(t) * r, sin(t) * r, 0); }
If z is our vertical axis, then z is always zero here.
Then, choose a rotation matrix R using a vector math library (I'm not sure which one you are using). You can usually create a euler angle representation. Rotate it by some random fixed amount about the x and y axes. This will rotate the circle so we put all the electrons on a random circular orbit around the origin. This is using Eigen:
Transform t = AngleAxis(rand() * 2 * M_PI, Vector3d(0, 1, 0))
* AngleAxis(rand() * 2 * M_PI, Vector3d(1, 0, 0));
Putting it together:
Vector3d createPoint(const Atom& atom, double t) {
auto xform = AngleAxis(rand() * 2 * M_PI, Vector3d(0, 1, 0))
* AngleAxis(rand() * 2 * M_PI, Vector3d(1, 0, 0));
Vector3d electronPoint = xform * circle(t, atom.shellRadius());
return electronPoint;
}
Finally, you can add all the points together for all the atoms in your original array
vector<Vector3d> getPoints() {
vector<shared_ptr<Atom>> atoms;
// construct your atoms depending on the molecule
// eg.
atom.push_back(make_shared<Electron>(1)); // Create an electron on shell 1
atom.push_back(make_shared<Neutron>()); // Create a neutron, which will be shell 0
vector<Vector3d> points;
for (const auto& atom : atoms) {
// instead of rand() you could use some scaling
// of the current time to get an animation with the shells orbiting,
points.push_back(createPoint(atom, rand()));
}
}
Finally, you can then pass this vector of points to irrlicht. You probably want to generate this every frame if you desire an animation.
Good luck! Irrlicht probably has a built-in matrix library which may also help. If you really want to speed things along, you could create custom vertex shaders which do all this math on the GPU, and then simply pass in electrons. This won't be necessary for any known molecules, though :). You also may want a fixed offset per electron. All of this could be done by adding parameters to the Electron and Neutron constructors, to change the start of their orbit.

Determining angular velocity required to adjust orientation based on Quaternions

Problem:
I have an object in 3D space that exists at a given orientation. I need to reorient the object to a new orientation. I'm currently representing the orientations as quaternions, though this is not strictly necessary.
I essentially need to determine the angular velocity needed to orient the body into the desired orientation.
What I'm currently working with looks something like the following:
Psuedocode:
// 4x4 Matrix containing rotation and translation
Matrix4 currentTransform = GetTransform();
// Grab the 3x3 matrix containing orientation only
Matrix3 currentOrientMtx = currentTransform.Get3x3();
// Build a quat based on the rotation matrix
Quaternion currentOrientation(currentOrientMtx);
currentOrientation.Normalize();
// Build a new matrix describing our desired orientation
Vector3f zAxis = desiredForward;
Vector3f yAxis = desiredUp;
Vector3f xAxis = yAxis.Cross(zAxis);
Matrix3 desiredOrientMtx(xAxis, yAxis, zAxis);
// Build a quat from our desired roation matrix
Quaternion desiredOrientation(desiredOrientMtx);
desiredOrientation.Normalize();
// Slerp from our current orientation to the new orientation based on our turn rate and time delta
Quaternion slerpedQuat = currentOrientation.Slerp(desiredOrientation, turnRate * deltaTime);
// Determine the axis and angle of rotation
Vector3f rotationAxis = slerpedQuat.GetAxis();
float rotationAngle = slerpedQuat.GetAngle();
// Determine angular displacement and angular velocity
Vector3f angularDisplacement = rotationAxis * rotationAngle;
Vector3f angularVelocity = angularDisplacement / deltaTime;
SetAngularVelocity(angularVelocity);
This essentially just sends my object spinning to oblivion. I have verified that the desiredOrientMtx I constructed via the axes is indeed the correct final rotation transformation. I feel like I'm missing something silly here.
Thoughts?
To calculate angular velocity, your turnRatealready provides the magnitude (rads/sec), so all you really need is the axis of rotation. That is just given by GetAxis( B * Inverse(A) ). GetAngle of that same quantity would give the total angle to travel between the two. See 'Difference' between two quaternions for further explanation.
SetAngularVelocity( Normalize( GetAxis( B * Inverse(A)) ) * turnRate )
You need to set the angular velocity to 0 at some point (when you reach your goal orientation). One way to do this is by using a quaternion distance. Another simpler way is by checking against the amount of time taken. Finally, you can check the angle between two quats (as discussed above) and check if that is close to 0.
float totalAngle = GetAngle( Normalize( endingPose * Inverse( startingPose ) ) );
if( fabs( totalAngle ) > 0.0001 ) // some epsilon
{
// your setting angular velocity code here
SetAngularVelocity(angularVelocity);
}
else
{
SetAngularVelocity( Vector3f(0) );
// Maybe, if you want high accuracy, call SetTransform here too
}
But, really, I don't see why you don't just use the Slerp to its fullest. Instead of relying on the physics integrator (which can be imprecise) and relying on knowing when you've reached your destination (which is somewhat awkward), you could just move the object frame-by-frame since you know the motion.
Quaternion startingPose;
Quaternion endingPose;
// As discussed earlier...
Quaternion totalAngle = Quaternion.AngleBetween( startingPose, endingPose );
// t is set to 0 whenever you start a new motion
t += deltaT;
float howFarIn = (turnRate * t) / totalAngle;
SetCurrentTransform( startingPose.Slerp( endingPose, howFarIn ) );
See Smooth rotation with quaternions for some discussion on that.

How to convert the positions of connected joints to relative delta rotations

I'm currently implementing a C++ solution to track motion of multiple objects. In that I have tracked points of those objects in a frame sequences such that multiple points in each frame. As a result of that I have x, y, z coordinates of those points of the entire frame sequence. By studying an already generated model I understood it consists of a joints system which move relative to each other. Every joint has a parent and their movements are written relative to its parent in Quaternion format. Therefore, I want to convert my x,y,z coordinates, which are in 3D space relative to same origin, to quaternion format which are written as relative to its parent. I can then use the quaternions to animate them.
I don't understand how to calculate the angle that it requires. Can you please provide me a sample code (in c++) or any useful resources to overcome this problem.
So we have a system of connected joints and we want to find out the the relative delta rotation of the joints from one frame to another. I'll call the relative rotation the local rotation since the "relative rotation" on it's own doesn't tell us what it's relative to. (Is it relative to an object, to the center of the universe, etc?)
Assumptions
I'm going to assume a tree structure of joints so that each joint only has one parent and we only have one root joint (without parents). If you have several parents per joint you should be able to use the same solution, but you'll then each joint will one relative rotation per parent and you need to do the calcualtion once for each parent. If you have several joints without parents then each one can be thought of as a root in it's own tree made up of the connected joints.
I'll assume you have a quaternion math library that can; create from an axis and an angle, set to identity, inverse, and accumulate quaternions. If you don't you should find all the info you need to implement them from Wikipedia or Google.
Calulating the rotations
The code below first calculates the local rotations of the joint for the start and the end of the frame. It does this calculation using two vector; the vector from it's parent and the vector from the grandparent to the parent. Then in order to calculate the delta rotation it uses the inverted start rotation to "remove" the start rotation from the end rotation by applying it's inverse rotation. So we end up with the local delta rotation for that frame.
For the first two levels of the joint hierarchy we have special cases which we can solve directly.
Pseudocode
The out parameter is a multidimensional array named result.
NB: startPosition, endPosition, parentStartPosition, parentEndPosition, grandParentStartPosition, grandParentStartPosition all have to be updated for each iteration of the loops. That update is not shown in order to focus on the core of the problem.
for each frame {
for each joint {
if no parent {
// no parent means no local rotation
result[joint,frame] = identityQuaternion
}
else {
startLink = startPosition - parentStartPosition
endLink = endPosition - parentEndPosition
if no grandParent {
// no grand parent - we can calculate the local rotation directly
result[joint,frame] = QuaternionFromVectors( startLink, endLink )
}
else {
parentStartLink = parentStartPosition - grandParentStartPosition
parentEndLink = parentEndPosition - grandParentEndPosition
// calculate the local rotations
// = the difference in rotation between parent link and child link
startRotation = QuaternionFromVectors( parentStartLink, startLink )
endRotation = QuaternionFromVectors( parentEndLink, endLink )
// calculate the delta local rotation
// = the difference between start and end local rotations
invertedStartRotation = Inverse( startRotation )
deltaRotation = invertedStartRotation.Rotate( endRotation )
result[joint,frame] = deltaRotation
}
}
}
}
QuaternionFromVectors( fromVector, toVector )
{
axis = Normalize( fromVector.Cross( toVector ) )
angle = Acos( fromVector.Dot( toVector ) )
return Quaternion( axis, angle )
}
C++ implementation
Below is an untested recursive implementation in C++. For each frame we start at the root of our JointData tree and then traverse the tree by recursivly calling the JointData::calculateRotations() function.
In order to make the code easier to read I have an accessor from the joint tree nodes JointData to the FrameData. You probably don't want to have such a direct dependency in your implementation.
// Frame data holds the individual frame data for a joint
struct FrameData
{
Vector3 m_positionStart;
Vector3 m_positionEnd;
// this is our unknown
Quaternion m_localDeltaRotation;
}
class JointData
{
public:
...
JointData *getChild( int index );
int getNumberOfChildren();
FrameData *getFrame( int frameIndex );
void calculateDeltaRotation( int frameIndex, JointData *parent = NULL,
Vector3& parentV1 = Vector3(0),
Vector3& parentV2 = Vector3(0) );
...
}
void JointData::calculateDeltaRotation( int frameIndex, JointData *parent,
Vector3& parentV1, Vector3& parentV2 )
{
FrameData *frameData = getFrame( frameIndex );
if( !parent )
{
// this is the root, it has no local rotation
frameData->m_localDeltaRotation.setIdentity();
return;
}
FrameData *parentFrameData = parent->getFrame( frameIndex );
// calculate the vector from our parent
// for the start (v1) and the end (v2) of the frame
Vector3 v1 = frameData->m_positionStart - parentFrameData->m_positionStart;
Vector3 v2 = frameData->m_positionEnd - parentFrameData->m_positionEnd;
if( !getParent()->getParent() )
{
// child of the root is a special case,
// we can calculate it's rotation directly
frameData->m_localDeltaRotation = calculateQuaternion( v1, v2 );
}
else
{
// calculate start and end rotations
// apply inverse start rotation to end rotation
Quaternion startRotation = calculateQuaternion( parentV1, v1 );
Quaternion endRotation = calculateQuaternion( parentV2, v2 );
Quaternion invStartRot = startRotation.inverse();
frameData->m_localDeltaRotation = invStartRot.rotate( endRotation );
}
for( int i = 0; i < getNumberOfChildren(); ++i )
{
getChild( i )->calculateRotations( frameIndex, this, v1, v2 );
}
}
// helper function to calulate a quaternion from two vector
Quaternion calculateQuaternion( Vector3& fromVector, Vector3& toVector )
{
float angle = acos( fromVector.dot( toVector ) );
Vector3 axis = fromVector.cross( toVector );
axis.normalize();
return Quaternion( axis, angle );
}
The code is written for readability and not to be optimal.
Point3d Controller::calRelativeToParent(int parentID,Point3d point,int frameID){
if(parentID == 0){
QUATERNION temp = calChangeAxis(-1,parentID,frameID);
return getVect(multiplyTwoQuats(multiplyTwoQuats(temp,getQuat(point)),getConj(temp)));
}else{
Point3d ref = calRelativeToParent(originalRelativePointMap[parentID].parentID,point,frameID);
QUATERNION temp = calChangeAxis(originalRelativePointMap[parentID].parentID,parentID,frameID);
return getVect(multiplyTwoQuats(multiplyTwoQuats(temp,getQuat(ref)),getConj(temp)));
}}
QUATERNION Controller::calChangeAxis(int parentID,int qtcId,int frameID){ //currentid = id of the position of the orientation to be changed
if(parentID == -1){
QUATERNION out = multiplyTwoQuats(quatOrigin.toChange,originalRelativePointMap[qtcId].orientation);
return out;
}
else{
//QUATERNION temp = calChangeAxis(originalRelativePointMap[parentID].parentID,qtcId,frameID);
//return multiplyTwoQuats(finalQuatMap[frameID][parentID].toChange,temp);
return multiplyTwoQuats(finalQuatMap[frameID][parentID].toChange,originalRelativePointMap[qtcId].orientation);
}}
This is the algorithm I have used. I first calculated the relative vector of each frame wrt to it's parent. Here parent ID of root is 0. Then I calculated the relative vector in the model for each joint. This is called recursively.