void dCustomTransformController::PostUpdate(dCustomTransformManager* const manager, dFloat timestep) const
{
if (m_calculateLocalTransform) {
dMatrix parentMatrixPool[128];
const dSkeletonBone* stackPool[128];
int stack = 1;
stackPool[0] = this;
parentMatrixPool[0] = dGetIdentityMatrix();
while (stack) {
dMatrix matrix;
stack--;
dMatrix parentMatrix(parentMatrixPool[stack]);
const dSkeletonBone* const bone = stackPool[stack];
NewtonBodyGetMatrix(bone->GetBody(), &matrix[0][0]);
manager->OnUpdateTransform(bone, matrix * parentMatrix * bone->GetBindMatrix());
parentMatrix = matrix.Inverse();
for (dList<dSkeletonBone>::dListNode* ptrNode = bone->GetFirst(); ptrNode; ptrNode = ptrNode->GetNext()) {
parentMatrixPool[stack] = parentMatrix;
stackPool[stack] = &ptrNode->GetInfo();
stack++;
}
}
}
}
DemoEntity::DemoEntity(DemoEntityManager& world, const dScene* scene, dScene::dTreeNode* rootSceneNode, dTree<DemoMesh*, dScene::dTreeNode*>& meshCache, DemoEntityManager::EntityDictionary& entityDictionary, DemoEntity* parent)
:dClassInfo()
,dHierarchy<DemoEntity>()
,m_matrix(GetIdentityMatrix())
,m_curPosition (0.0f, 0.0f, 0.0f, 1.0f)
,m_nextPosition (0.0f, 0.0f, 0.0f, 1.0f)
,m_curRotation (1.0f, 0.0f, 0.0f, 0.0f)
,m_nextRotation (1.0f, 0.0f, 0.0f, 0.0f)
,m_lock (0)
,m_mesh (NULL)
{
// add this entity to the dictionary
entityDictionary.Insert(this, rootSceneNode);
// if this is a child mesh set it as child of th entity
dMatrix parentMatrix (GetIdentityMatrix());
if (parent) {
Attach (parent);
dScene::dTreeNode* parentNode = scene->FindParentByType(rootSceneNode, dSceneNodeInfo::GetRttiType());
dSceneNodeInfo* parentInfo = (dSceneNodeInfo*) parentNode;
parentMatrix = parentInfo->GetTransform();
}
dSceneNodeInfo* info = (dSceneNodeInfo*) scene->GetInfoFromNode (rootSceneNode);
// SetMatrix(info->GetTransform() * parentMatrix.Inverse4x4());
dMatrix matrix (info->GetTransform() * parentMatrix.Inverse4x4());
dQuaternion rot (matrix);
// set the matrix twice in oder to get cur and next position
SetMatrix(world, rot, matrix.m_posit);
SetMatrix(world, rot, matrix.m_posit);
// if this node has a mesh, find it and attach it to this entity
dScene::dTreeNode* meshNode = scene->FindChildByType(rootSceneNode, dMeshNodeInfo::GetRttiType());
if (meshNode) {
DemoMesh* mesh = meshCache.Find(meshNode)->GetInfo();
SetMesh(mesh);
}
// add all of the children nodes as child nodes
for (void* child = scene->GetFirstChild(rootSceneNode); child; child = scene->GetNextChild (rootSceneNode, child)) {
dScene::dTreeNode* node = scene->GetNodeFromLink(child);
dNodeInfo* info = scene->GetInfoFromNode(node);
if (info->GetTypeId() == dSceneNodeInfo::GetRttiType()) {
new DemoEntity (world, scene, node, meshCache, entityDictionary, this);
}
}
}
Matrix3 plMaxNodeBase::GetWorldToParent(TimeValue t)
{
// This may look back-ass-ward, but that's only because it
// is. If we've got inheritance filtering on, then our localtoworld
// is no longer parentl2w * l2p, because we'll be ignoring
// some of our parent's transform. More precisely, we'll be
// clobbering parts of the product of our parent's current transform
// and our current local to parent. So we're going to calculate
// a parent to world transform here that would get us to the
// right point and orientation in space, even though it has
// little or nothing to do with our parent's real transform.
// Note that we only go through this charade if we've got
// filtering of inheritance active for this node.
plMaxNodeBase* parent = (plMaxNodeBase*)GetParentNode();
if( !GetFilterInherit() )
return parent->GetWorldToLocal(t);
// l2w = l2p * parentL2W
// l2w * parentW2L = l2p
// parentW2L = w2l * l2p
Point3 pos;
float rot[4];
ScaleValue scl;
Interval posInv;
Interval rotInv;
Interval sclInv;
Matrix3Indirect parentMatrix(parent->GetNodeTM(t));
TMComponentsArg cmpts(&pos, &posInv, rot, &rotInv, &scl, &sclInv);
GetTMController()->GetLocalTMComponents(t, cmpts, parentMatrix);
Quat q;
if( cmpts.rotRep == TMComponentsArg::RotationRep::kQuat )
q = Quat(rot);
else
EulerToQuat(rot, q, cmpts.rotRep);
Matrix3 l2p(true);
l2p.PreTranslate(pos);
PreRotateMatrix(l2p, q);
l2p.PreScale(scl.s);
PreRotateMatrix(l2p, scl.q);
Matrix3 w2l = GetWorldToLocal(t);
return w2l * l2p;
}
CustomPointToPoint::CustomPointToPoint(const dVector& pivotInChildInGlobalSpace, const dVector& pivotInParentInGlobalSpace, NewtonBody* const child, NewtonBody* const parent)
:CustomJoint(6, child, parent)
{
dVector dist(pivotInChildInGlobalSpace - pivotInParentInGlobalSpace);
m_distance = dSqrt(dist % dist);
dMatrix childMatrix(dGetIdentityMatrix());
dMatrix parentMatrix(dGetIdentityMatrix());
childMatrix.m_posit = pivotInChildInGlobalSpace;
parentMatrix.m_posit = pivotInParentInGlobalSpace;
childMatrix.m_posit.m_w = 1.0f;
parentMatrix.m_posit.m_w = 1.0f;
dMatrix dummy;
CalculateLocalMatrix(childMatrix, m_localMatrix0, dummy);
CalculateLocalMatrix(parentMatrix, dummy, m_localMatrix1);
}
void NodeSet::sort() const
{
if (m_isSorted)
return;
unsigned nodeCount = m_nodes.size();
if (nodeCount < 2) {
const_cast<bool&>(m_isSorted) = true;
return;
}
if (nodeCount > traversalSortCutoff) {
traversalSort();
return;
}
bool containsAttributeNodes = false;
WillBeHeapVector<NodeSetVector> parentMatrix(nodeCount);
for (unsigned i = 0; i < nodeCount; ++i) {
NodeSetVector& parentsVector = parentMatrix[i];
Node* n = m_nodes[i].get();
parentsVector.append(n);
if (n->isAttributeNode()) {
n = toAttr(n)->ownerElement();
parentsVector.append(n);
containsAttributeNodes = true;
}
while ((n = n->parentNode()))
parentsVector.append(n);
}
sortBlock(0, nodeCount, parentMatrix, containsAttributeNodes);
// It is not possible to just assign the result to m_nodes, because some
// nodes may get dereferenced and destroyed.
WillBeHeapVector<RefPtrWillBeMember<Node> > sortedNodes;
sortedNodes.reserveInitialCapacity(nodeCount);
for (unsigned i = 0; i < nodeCount; ++i)
sortedNodes.append(parentMatrix[i][0]);
const_cast<WillBeHeapVector<RefPtrWillBeMember<Node> >&>(m_nodes).swap(sortedNodes);
}
void NodeSet::sort() const
{
if (m_isSorted)
return;
unsigned nodeCount = m_nodes.size();
if (nodeCount < 2) {
m_isSorted = true;
return;
}
if (nodeCount > traversalSortCutoff) {
traversalSort();
return;
}
bool containsAttributeNodes = false;
Vector<Vector<Node*>> parentMatrix(nodeCount);
for (unsigned i = 0; i < nodeCount; ++i) {
Vector<Node*>& parentsVector = parentMatrix[i];
Node* node = m_nodes[i].get();
parentsVector.append(node);
if (is<Attr>(*node)) {
node = downcast<Attr>(*node).ownerElement();
parentsVector.append(node);
containsAttributeNodes = true;
}
while ((node = node->parentNode()))
parentsVector.append(node);
}
sortBlock(0, nodeCount, parentMatrix, containsAttributeNodes);
// It is not possible to just assign the result to m_nodes, because some nodes may get dereferenced and destroyed.
Vector<RefPtr<Node>> sortedNodes;
sortedNodes.reserveInitialCapacity(nodeCount);
for (unsigned i = 0; i < nodeCount; ++i)
sortedNodes.append(parentMatrix[i][0]);
m_nodes = WTF::move(sortedNodes);
m_isSorted = true;
}
//-----------------------------------------------------------
/// Calculates bone to world transformation.
///
/// BoneToWorldMatrix calculates the matrix which transforms a
/// vertex from the bone space to the world space.
///
/// \param head The head vector (relative to tail of parent; in bone space of parent).
/// \param tail The tail vector (relative to tail of parent; in bone space of parent).
/// \param roll The roll value in radiant.
/// \param pParent The parent bone (null if no parent exists).
/// \return Transformation matrix.
//-----------------------------------------------------------
Matrix ComputeBoneToWorldMatrix(Vector head, Vector tail, float roll, const Bone* pParent)
{
//
// Matrix of parent bone
//
Matrix parentMatrix(IdentityMatrix);
if (pParent != 0)
parentMatrix = pParent->boneToWorldMatrix;
//
// Translate to tail of parent bone.
// (Remember: Child-bone is always given relative to tail of
// parent-bone.)
//
Matrix lengthTranslation(IdentityMatrix);
if(pParent != 0)
lengthTranslation.SetTranslation(Vector(0.0, pParent->length, 0.0));
//
// Translate from tail of parent bone to head of current bone
//
Matrix rootTranslation(IdentityMatrix);
rootTranslation.SetTranslation(head);
//
// Rotation of bone
//
Matrix rotation = BoneRotation(head, tail, roll);
// Cumulate matrices.
// Read from right-to-left:
// (Assume: Vertex is given in coordinate system of current bone.)
// rootTranslation * rotation ... Transform vertex to coordinates of parent bone
// (relative to tip/joint of parent bone).
// lengthTranslation ............ Translate vertex to coordinates of parent bone
// (relative to root of parent bone).
// parentMatrix ................. Transform vertex to world coordinates.
return parentMatrix * lengthTranslation * rootTranslation * rotation;
}
void NodeSet::sort() const
{
if (m_isSorted)
return;
unsigned nodeCount = m_nodes.size();
if (nodeCount < 2) {
const_cast<bool&>(m_isSorted) = true;
return;
}
bool containsAttributeNodes = false;
Vector<Vector<Node*> > parentMatrix(nodeCount);
for (unsigned i = 0; i < nodeCount; ++i) {
Vector<Node*>& parentsVector = parentMatrix[i];
Node* n = m_nodes[i].get();
parentsVector.append(n);
if (n->isAttributeNode()) {
n = static_cast<Attr*>(n)->ownerElement();
parentsVector.append(n);
containsAttributeNodes = true;
}
while ((n = n->parent()))
parentsVector.append(n);
}
sortBlock(0, nodeCount, parentMatrix, containsAttributeNodes);
// It is not possible to just assign the result to m_nodes, because some nodes may get dereferenced and destroyed.
Vector<RefPtr<Node> > sortedNodes;
sortedNodes.reserveCapacity(nodeCount);
for (unsigned i = 0; i < nodeCount; ++i)
sortedNodes.append(parentMatrix[i][0]);
const_cast<Vector<RefPtr<Node> >& >(m_nodes).swap(sortedNodes);
}
void update()
{
// Initialize function/variables
glm::mat4 planetTranslation(1.0f);
glm::mat4 planetRotation(1.0f);
glm::mat4 planetScaling(1.0f);
static float angle = 0.0;
float dt = getDT(); // if you have anything moving, use dt.
// Check if we need to stop the cube's rotation
if( stopSpin == true )
{
// If so, simulate as if no time has gone by (cube will stay wherever
// it already is)
dt = 0;
}
// Check if we need to reverse the spinning of the cube
if( reverseSpin == true )
{
angle -= dt * M_PI/2; //move through 90 degrees a second
}
else
{
angle += dt * M_PI/2; //move through 90 degrees a second
}
// Calculate movement of celestial objects
for( unsigned int i = 0; i < spaceObjects.size(); i++ )
{
// Find the parent planet
int parentPlanet = spaceObjects[i].objectParentPlanet;
glm::mat4 parentMatrix(1.0f);
spaceObjects[i].objectModel = glm::mat4(1.0f);
if( parentPlanet != 0 )
{
parentMatrix = spaceObjects[parentPlanet].objectModel;
}
// Update position
spaceObjects[i].position = glm::vec3(spaceObjects[i].objectRevolutionRadius
* sin( angle * spaceObjects[i].objectRevolutionSpeed ),
0.0,
spaceObjects[i].objectRevolutionRadius
* cos( angle * spaceObjects[i].objectRevolutionSpeed ));
spaceObjects[i].xPos = spaceObjects[i].objectRevolutionRadius
* sin( angle * spaceObjects[i].objectRevolutionSpeed );
spaceObjects[i].yPos = 0.0;
spaceObjects[i].zPos = spaceObjects[i].objectRevolutionRadius
* cos( angle * spaceObjects[i].objectRevolutionSpeed );
// Calculate translation matrix for object
planetTranslation = glm::translate( parentMatrix,
glm::vec3(spaceObjects[i].objectRevolutionRadius
* sin( angle * spaceObjects[i].objectRevolutionSpeed ),
0.0,
spaceObjects[i].objectRevolutionRadius
* cos( angle * spaceObjects[i].objectRevolutionSpeed )));
// Calculation scaling matrix for each object
planetScaling = glm::scale( glm::mat4(1.0f),
glm::vec3(spaceObjects[i].objectRadius) );
/*
// Calculate the rotation matrix for object
if( spaceObjects[i].objectIdentifier != 'R' )
{
planetRotation = glm::rotate( glm::mat4(1.0f),
angle * spaceObjects[i].objectSelfRevolution,
glm::vec3( 0.0, 1.0, 0.0 ) );
}
else
{
planetRotation = glm::rotate( glm::mat4(1.0f),
0.0f,
glm::vec3( 0.0, 1.0, 0.0 ) );
}
*/
// Calculate the rotation matrix for object
planetRotation = glm::rotate( glm::mat4(1.0f),
angle * spaceObjects[i].objectSelfRevolution,
glm::vec3( 0.0, 1.0, 0.0 ) );
// Calculate the model matrix - remember, order matters - planet
spaceObjects[i].objectModel *= planetTranslation * planetRotation * planetScaling;
}
// Update the state of the scene
glutPostRedisplay();//call the display callback
}
