TEST(TestNodeAAB, TestAABSimple) {
MockLoader mock;
auto node = std::make_shared<FTShaderNode<MockShader>>();
node->setSize(glm::vec3(34, 88, 22));
node->updateMatrices();
node->updateAAB(node->getTransformMatrix());
EXPECT_EQ(node->getAABCenter(), glm::vec3(17, 44, 11));
EXPECT_EQ(node->getAABHalfExtents(), glm::vec3(17, 44, 11));
node->setPosition(glm::vec3(542, 2, 7));
node->updateMatrices();
node->updateAAB(node->getTransformMatrix());
EXPECT_EQ(node->getAABCenter(), glm::vec3(17 + 542, 44 + 2, 11 + 7.0f));
EXPECT_EQ(node->getAABHalfExtents(), glm::vec3(17, 44, 11));
node->setScale(glm::vec3(5, 2, 7));
node->updateMatrices();
node->updateAAB(node->getTransformMatrix());
EXPECT_EQ(node->getAABCenter(), glm::vec3(17*5 + 542, 44*2 + 2, 11*7 + 7.0f));
EXPECT_EQ(node->getAABHalfExtents(), glm::vec3(17*5, 44*2, 11*7));
auto quat = glm::angleAxis((float)M_PI_2, glm::vec3(0, 0, 1));
node->setRotationQuaternion(quat);
node->updateMatrices();
node->updateAAB(node->getTransformMatrix());
EXPECT_EQ(node->getAABCenter(), glm::vec3(542 - 44 * 2, 2+17 * 5, 11 * 7 + 7.0f));
EXPECT_EQ(node->getAABHalfExtents(), glm::vec3(44 * 2, 17 * 5, 11 * 7));
}
TEST(TestNodeAAB, TestAABAnchorPoint) {
MockLoader mock;
auto node = std::make_shared<FTShaderNode<MockShader>>();
node->setAnchorPoint(glm::vec3(0.3f, 0.5f, 0.7f));
node->setSize(glm::vec3(34, 88, 22));
node->updateMatrices();
node->updateAAB(node->getTransformMatrix());
expectVectorEqual(node->getAABCenter(), glm::vec3(17 - 0.3f * 34, 0, 11 - 0.7f * 22));
expectVectorEqual(node->getAABHalfExtents(), glm::vec3(17, 44, 11));
node->setPosition(glm::vec3(542, 2, 7));
node->updateMatrices();
node->updateAAB(node->getTransformMatrix());
expectVectorEqual(node->getAABCenter(), glm::vec3(17 - 0.3f * 34 + 542, 2, 11 - 0.7f * 22 + 7.0f));
expectVectorEqual(node->getAABHalfExtents(), glm::vec3(17, 44, 11));
node->setScale(glm::vec3(5, 2, 7));
node->updateMatrices();
node->updateAAB(node->getTransformMatrix());
expectVectorEqual(node->getAABCenter(), glm::vec3((17 - 0.3f * 34) * 5 + 542, 2, (11 - 0.7f * 22) * 7 + 7.0f), 0.00001f);
expectVectorEqual(node->getAABHalfExtents(), glm::vec3(17 * 5, 44 * 2, 11 * 7));
auto quat = glm::angleAxis((float)M_PI_2, glm::vec3(0, 0, 1));
node->setRotationQuaternion(quat);
node->updateMatrices();
node->updateAAB(node->getTransformMatrix());
expectVectorEqual(node->getAABCenter(), glm::vec3(542, 2 + (17 - 0.3f * 34) * 5, (11 - 0.7f * 22) * 7 + 7.0f), 0.00001f);
expectVectorEqual(node->getAABHalfExtents(), glm::vec3(44 * 2, 17 * 5, 11 * 7));
}
void MotionGraph::advance()
{
currentFrame++;
if (currentFrame >= clips[currentEdge]->GetNumFrames() - 1) // is the last frame
{
double matrix[16];
getTransformMatrix(matrix, edges[currentEdge].theta, edges[currentEdge].x0, edges[currentEdge].z0);
glPushMatrix();
glLoadIdentity();
glMultMatrixd(transformMatrix);
glMultMatrixd(matrix);
//glMultMatrixd(transformMatrix);
glGetDoublev(GL_MODELVIEW_MATRIX, transformMatrix);
glPopMatrix();
if (edges[currentEdge].rest != 0 && nodes[edges[currentEdge].src].label == targetLabel)
targetLabel = restLabel;
int nodeId = edges[currentEdge].dst;
currentEdge = nodes[nodeId].next[targetLabel];
currentFrame = 0;
//printf("edge(%d): %d(%d) --> %d(%d)\n", currentEdge, nodes[edges[currentEdge].src].motionIdx, nodes[edges[currentEdge].src].frameIdx,
// nodes[edges[currentEdge].dst].motionIdx, nodes[edges[currentEdge].dst].frameIdx);
}
}
Vector3 CollisionManager::convertToModelSpace(const Vector3& point, SceneObject* model) {
Reference<Matrix4*> modelMatrix = getTransformMatrix(model);
Vector3 transformedPoint = point * *modelMatrix;
return transformedPoint;
}
// -----------------------------------------------------------------------------------------
bool CPepeEngineCamera::isVisible(const CPepeEngineVector3& vert, FrustumPlane* culledBy) const
{
// Make any pending updates to the calculated frustum planes
CPepeEngineFrustum::updateFrustumPlanes(getTransformMatrix());
// For each plane, see if all points are on the negative side
// If so, object is not visible
for (int plane = 0; plane < 6; ++plane) {
// Skip far plane if infinite view frustum
if (plane == FRUSTUM_PLANE_FAR && m_fFarDist == 0) {
continue;
}
if (m_frustumPlanes[plane].getSide(vert) == CPepeEnginePlane::NEGATIVE_SIDE) {
// ALL corners on negative side therefore out of view
if (culledBy) {
*culledBy = (FrustumPlane)plane;
}
return false;
}
}
return true;
}
// -----------------------------------------------------------------------------------------
bool CPepeEngineCamera::isVisible(const CPepeEngineSphere& sphere, FrustumPlane* culledBy) const
{
// Make any pending updates to the calculated frustum planes
CPepeEngineFrustum::updateFrustumPlanes(getTransformMatrix());
// For each plane, see if sphere is on negative side
// If so, object is not visible
for (int plane = 0; plane < 6; ++plane) {
// Skip far plane if infinite view frustum
if (plane == FRUSTUM_PLANE_FAR && m_fFarDist == 0) {
continue;
}
// If the distance from sphere center to plane is negative, and 'more negative'
// than the radius of the sphere, sphere is outside frustum
if (m_frustumPlanes[plane].getDistance(sphere.getCenter()) < -sphere.getRadius()) {
// ALL corners on negative side therefore out of view
if (culledBy) {
*culledBy = (FrustumPlane)plane;
}
return false;
}
}
return true;
}
virtual void destroy()
{
setState(State_Fadeout);
setRoutine(RCID_Null);
atmGetFluidModule()->addFluid(getModel(), getTransformMatrix());
atmPlaySE(m_explosion_channel, m_explosion_se, getPositionAbs(), true);
}
float Viewport::getZmin() const
{
float zmin = INFINITY;
for (auto p : findChildren<DepthImageItem*>())
zmin = fmin((getTransformMatrix() * p->pos).z() - p->size.z()/2,
zmin);
return zmin;
}
void Sprite::visit()
{
m_frame->setProgram(getProgram());
m_frame->setAnchontPoint(&getAnchontPoint());
m_frame->setContentSize(&getContentSize());
m_frame->setMatrix(getTransformMatrix());
m_frame->visit();
}
float Viewport::getZmax() const
{
float zmax = -INFINITY;
for (auto p : findChildren<DepthImageItem*>())
zmax = fmax((getTransformMatrix() * p->pos).z() + p->size.z()/2,
zmax);
return zmax;
}
virtual void draw()
{
vec4 diffuse = getDiffuseColor();
vec4 glow = getGlowColor();
vec4 light = m_light_color;
vec4 flash = getDamageColor();
if(getState()==State_Fadein) {
float32 s = (float32)m_st_frame / FADEIN_TIME;
float shininess = diffuse.w;
diffuse *= stl::min<float32>(s*2.0f, 1.0f);
diffuse.w = shininess;
glow *= stl::max<float32>(s*2.0f-1.0f, 0.0f);
light *= s;
}
else if(getState()==State_Fadeout) {
float32 s = 1.0f - ((float32)m_st_frame / FADEOUT_TIME);
light *= s;
}
if(m_light_radius > 0.0f) {
if(atmGetConfig()->lighting_level>=atmE_Lighting_Medium) {
PointLight l;
l.setPosition(getPositionAbs() + vec3(0.0f, 0.0f, m_light_radius*0.5f));
l.setColor(light);
l.setRadius(m_light_radius);
atmGetLightPass()->addLight(l);
}
else {
flash += light*0.05f;
glow *= 2.0f;
}
}
if(m_state!=State_Fadeout) {
PSetInstance inst;
inst.diffuse = diffuse;
inst.glow = glow;
inst.flash = flash;
inst.elapsed = getPastTime();
inst.appear_radius = inst.elapsed * 0.004f;
inst.transform = inst.rotate = getTransformMatrix();
atmGetFluidPass()->addParticles(getModel(), inst, computeNumParticles());
atmGetBloodStainPass()->addBloodstainParticles(getTransformMatrix(), getBloodStainParticles(), getNumBloodstainParticles());
}
}
Floor::Floor(GLuint shader, GLfloat width, GLfloat height, glm::vec3 position, std::shared_ptr<Renderer::Model> model, std::shared_ptr<Renderer::Frustum> frustum) {
this->m_name = "Floor";
this->m_shader = shader;
this->m_model = model;
this->m_frustum = frustum;
m_position = position;
m_scale = glm::vec3(width, 1, height);
m_transform = getTransformMatrix();
}
Ray CollisionManager::convertToModelSpace(const Vector3& rayOrigin, const Vector3& rayEnd, SceneObject* model) {
Reference<Matrix4*> modelMatrix = getTransformMatrix(model);
Vector3 transformedOrigin = rayOrigin * *modelMatrix;
Vector3 transformedEnd = rayEnd * *modelMatrix;
Vector3 norm = transformedEnd - transformedOrigin;
norm.normalize();
Ray ray(transformedOrigin, norm);
return ray;
}
float Canvas::getZmin() const
{
float zmin = INFINITY;
for (auto i : scene->items())
{
DepthImageItem* p = dynamic_cast<DepthImageItem*>(i);
if (p)
{
zmin = fmin(zmin, (getTransformMatrix() * p->pos).z() -
p->size.z()/2);
}
}
return zmin;
}
virtual void asyncupdate(float32 dt)
{
super::asyncupdate(dt);
transform::updateRotate(dt);
transform::updateTransformMatrix();
bloodstain::updateBloodstain(dt);
float32 rigid_scale = 1.0f;
if(getState()==State_Fadein) {
rigid_scale = ((float32)m_st_frame / FADEIN_TIME);
}
if(getState()!=State_Fadeout) {
collision::updateCollisionByParticleSet(getModel(), getTransformMatrix(), vec3(rigid_scale));
}
}
void CalBone::calculateBoundingBox()
{
if(!getCoreBone()->isBoundingBoxPrecomputed())
return;
CalVector dir = CalVector(1.0f,0.0f,0.0f);
dir*=getTransformMatrix();
m_boundingBox.plane[0].setNormal(dir);
dir = CalVector(-1.0f,0.0f,0.0f);
dir*=getTransformMatrix();
m_boundingBox.plane[1].setNormal(dir);
dir = CalVector(0.0f,1.0f,0.0f);
dir*=getTransformMatrix();
m_boundingBox.plane[2].setNormal(dir);
dir = CalVector(0.0f,-1.0f,0.0f);
dir*=getTransformMatrix();
m_boundingBox.plane[3].setNormal(dir);
dir = CalVector(0.0f,0.0f,1.0f);
dir*=getTransformMatrix();
m_boundingBox.plane[4].setNormal(dir);
dir = CalVector(0.0f,0.0f,-1.0f);
dir*=getTransformMatrix();
m_boundingBox.plane[5].setNormal(dir);
int i;
for(i=0;i< 6; i++)
{
CalVector position;
getCoreBone()->getBoundingData(i,position);
position*=getTransformMatrix();
position+=getTranslationBoneSpace();
int planeId;
for(planeId = 0; planeId < 6; ++planeId)
{
if(m_boundingBox.plane[planeId].eval(position) < 0.0f)
{
m_boundingBox.plane[planeId].setPosition(position);
}
}
}
}
void SceneNode::createDisplayList()
{
bool firstTime = false;
if(usesDisplayList)
{
if(hasDisplayList())
{
glCallList(this->displayListIndex);
}
else
{
firstTime = true;
displayListIndex = glGenLists(1);
glNewList(displayListIndex, GL_COMPILE);
}
}
glPushMatrix();
glMultMatrixf(getTransformMatrix());
if(hasAppearance)
{
addAppearanceToStack(this->appearance);
}
drawPrimitives();
for(unsigned int i = 0; i < descendants.size(); i++)
{
descendants.at(i)->createDisplayList();
}
if(hasAppearance)
{
removeAppearanceFromStack();
}
glPopMatrix();
if(usesDisplayList && firstTime)
{
glEndList();
}
}
// -----------------------------------------------------------------------------------------
bool CPepeEngineCamera::isVisible(const CPepeEngineAxisAlignedBox& bound, FrustumPlane* culledBy) const
{
// Null boxes always invisible
if (bound.isNull()) {
return false;
}
// Infinite boxes always visible
if (bound.isInfinite()) {
return true;
}
// Make any pending updates to the calculated frustum planes
CPepeEngineFrustum::updateFrustumPlanes(getTransformMatrix());
// Get centre of the box
CPepeEngineVector3 centre = bound.getCenter();
// Get the half-size of the box
CPepeEngineVector3 halfSize = bound.getHalfSize();
// For each plane, see if all points are on the negative side
// If so, object is not visible
for (int plane = 0; plane < 6; ++plane) {
// Skip far plane if infinite view frustum
if (plane == FRUSTUM_PLANE_FAR && m_fFarDist == 0) {
continue;
}
CPepeEnginePlane::Side side = m_frustumPlanes[plane].getSide(centre, halfSize);
if (side == CPepeEnginePlane::NEGATIVE_SIDE) {
// ALL corners on negative side therefore out of view
if (culledBy) {
*culledBy = (FrustumPlane)plane;
}
return false;
}
}
return true;
}
QPair<char, float> Viewport::getAxis() const
{
const auto M = getTransformMatrix();
const float threshold = 0.98;
const auto a = M.inverted() * QVector3D(0, 0, 1);
QList<QPair<char, QVector3D>> axes = {
{'x', QVector3D(1, 0, 0)},
{'y', QVector3D(0, 1, 0)},
{'z', QVector3D(0, 0, 1)}};
for (const auto v : axes)
{
float dot = fabs(QVector3D::dotProduct(a, v.second));
if (dot > threshold)
return QPair<char, float>(v.first, (dot - threshold) / (1 - threshold));
}
return QPair<char, float>('\0', 0);
}
void SceneNode::Display()
{
if(usingDL && this->usesDisplayList && this->hasDisplayList())
{
glCallList(this->displayListIndex);
}
else
{
glPushMatrix();
glMultMatrixf(getTransformMatrix());
if(hasAppearance)
{
addAppearanceToStack(this->appearance);
}
// TP2
if(hasAnimation)
{
animation->draw();
}
drawPrimitives();
for(unsigned int i = 0; i < descendants.size(); i++)
{
descendants.at(i)->Display();
}
if(hasAppearance)
{
removeAppearanceFromStack();
}
glPopMatrix();
}
}
kmMat4 Camera::getProjTransformMatrix()
{
kmMat4 result = getTransformMatrix();
kmMat4Multiply(&result, &mCamera.projection, &result);
return result;
}
void SceneTree::updateObjectTree( dp::sg::ui::ViewStateSharedPtr const& vs )
{
//
// first step: update node-local information
//
// update dirty object hints & masks
{
ObjectObserver::NewCacheData cd;
m_objectObserver->popNewCacheData( cd );
ObjectObserver::NewCacheData::const_iterator it, it_end = cd.end();
for( it=cd.begin(); it!=it_end; ++it )
{
ObjectTreeNode& node = m_objectTree[ it->first ];
node.m_localHints = it->second.m_hints;
node.m_localMask = it->second.m_mask;
m_objectTree.markDirty( it->first, ObjectTreeNode::DEFAULT_DIRTY );
}
}
// update dirty switch information
ObjectTreeIndexSet dirtySwitches;
m_switchObserver->popDirtySwitches( dirtySwitches );
if( !dirtySwitches.empty() )
{
ObjectTreeIndexSet::iterator it, it_end = dirtySwitches.end();
for( it=dirtySwitches.begin(); it!=it_end; ++it )
{
ObjectTreeIndex index = *it;
SwitchWeakPtr swp = m_objectTree.m_switchNodes[ index ];
DP_ASSERT( swp );
ObjectTreeIndex childIndex = m_objectTree[index].m_firstChild;
// counter for the i-th child
size_t i = 0;
while( childIndex != ~0 )
{
ObjectTreeNode& childNode = m_objectTree[childIndex];
DP_ASSERT( childNode.m_parentIndex == index );
bool newActive = swp->isActive( checked_cast<unsigned int>(i) );
if ( childNode.m_localActive != newActive )
{
childNode.m_localActive = newActive;
m_objectTree.markDirty( childIndex, ObjectTreeNode::DEFAULT_DIRTY );
}
childIndex = childNode.m_nextSibling;
++i;
}
}
}
// update all lods
if( !m_objectTree.m_LODs.empty() )
{
float scaleFactor = vs->getLODRangeScale();
const Mat44f& worldToView = vs->getCamera()->getWorldToViewMatrix();
std::map< ObjectTreeIndex, LODWeakPtr >::iterator it, it_end = m_objectTree.m_LODs.end();
for( it = m_objectTree.m_LODs.begin(); it != it_end; ++it )
{
ObjectTreeIndex index = it->first;
const ObjectTreeNode& node = m_objectTree[ index ];
const Mat44f modelToWorld = getTransformMatrix( node.m_transformIndex );
const Mat44f modelToView = modelToWorld * worldToView;
ObjectTreeIndex activeIndex = it->second->getLODToUse( modelToView, scaleFactor );
ObjectTreeIndex childIndex = m_objectTree[index].m_firstChild;
// counter for the i-th child
size_t i = 0;
while( childIndex != ~0 )
{
ObjectTreeNode& childNode = m_objectTree[childIndex];
DP_ASSERT( childNode.m_parentIndex == index );
bool newActive = activeIndex == i;
if ( childNode.m_localActive != newActive )
{
childNode.m_localActive = newActive;
m_objectTree.markDirty( childIndex, ObjectTreeNode::DEFAULT_DIRTY );
}
childIndex = childNode.m_nextSibling;
++i;
}
}
}
//
// second step: update resulting node-world information
//
UpdateObjectVisitor objectVisitor( m_objectTree, this );
PreOrderTreeTraverser<ObjectTree, UpdateObjectVisitor> objectTraverser;
objectTraverser.processDirtyList( m_objectTree, objectVisitor, ObjectTreeNode::DEFAULT_DIRTY );
m_objectTree.m_dirtyObjects.clear();
}
void Node::getTransformMatrix(float value[4][4])
{
SFMatrix mx;
getTransformMatrix(&mx);
mx.getValue(value);
}
// distance between two point clouds
// return := T(matrix), pa ~= T * pb
double MotionGraph::distance(PointCloud *pcA, PointCloud *pcB, double *matrix)
{
if (pcA->getNumPoints() != pcB->getNumPoints())
{
printf("Error: in MotionGraph::getTransformMatrix().\n");
exit(-1);
}
double numPoints = pcA->getNumPoints(); // the two numbers should be the same
vector *pa = pcA->getPoints();
vector *pb = pcB->getPoints();
double w = 1.0; // weight
double sumWeight = w * numPoints;
// calculate x_bar, x'_bar, z_bar, z'_bar
double xaBar = 0.0, zaBar = 0.0;
double xbBar = 0.0, zbBar = 0.0;
for (int i = 0; i < numPoints; i++)
{
xaBar += w * pa[i].x();
zaBar += w * pa[i].z();
}
for (int i = 0; i < numPoints; i++)
{
xbBar += w * pb[i].x();
zbBar += w * pb[i].z();
}
// calculate theta, x0, z0
double nom = 0.0;
double denom = 0.0;
for (int i = 0; i < numPoints; i++)
{
nom += w * (pa[i].x() * pb[i].z() - pb[i].x() * pa[i].z());
denom += w * (pa[i].x() * pb[i].x() + pa[i].z() * pb[i].z());
}
nom -= (xaBar * zbBar - xbBar * zaBar) / sumWeight;
denom -= (xaBar * xbBar + zaBar * zbBar) / sumWeight;
double theta = atan(nom / denom);
double x0 = (xaBar - xbBar * cos(theta) - zbBar * sin(theta)) / sumWeight;
double z0 = (zaBar + xbBar * sin(theta) - zbBar * cos(theta)) / sumWeight;
// sometimes you get wrong theta
// try opposite direction
double theta2 = theta + M_PI;
double x02 = (xaBar - xbBar * cos(theta2) - zbBar * sin(theta2)) / sumWeight;
double z02 = (zaBar + xbBar * sin(theta2) - zbBar * cos(theta2)) / sumWeight;
double m[16], m2[16];
getTransformMatrix(m, theta, x0, z0);
getTransformMatrix(m2, theta2, x02, z02);
// calculate distance
double dist = 0.0;
double dist2 = 0.0;
for (int i = 0; i < numPoints; i++)
{
vector tpbi = matMultVec3(m, pb[i]);
double dx = pa[i].x() - tpbi.x();
double dy = pa[i].y() - tpbi.y();
double dz = pa[i].z() - tpbi.z();
dist += w * (dx * dx + dy * dy + dz * dz);
vector tpbi2 = matMultVec3(m2, pb[i]);
double dx2 = pa[i].x() - tpbi2.x();
double dy2 = pa[i].y() - tpbi2.y();
double dz2 = pa[i].z() - tpbi2.z();
dist2 += w * (dx2 * dx2 + dy2 * dy2 + dz2 * dz2);
}
if (dist <= dist2)
{
if (matrix != NULL)
copyMatrix(matrix, m);
return dist;
}
else
{
if (matrix != NULL)
copyMatrix(matrix, m2);
return dist2;
}
}
urdf_traverser::EigenTransform urdf_traverser::getTransform(const LinkConstPtr& from_link, const LinkConstPtr& to_link)
{
return EigenTransform(getTransformMatrix(from_link, to_link));
}
