hkpShape* DestructibleHierarchy::buildShape(int rootNodeIdx)
{
if (rootNodeIdx == INVALID_NODE_INDEX)
{
rootNodeIdx = m_rootNodeIdx;
}
const DestructibleHierarchy::Node& node = m_nodes[rootNodeIdx];
hkArray<hkpShape*> shapes;
if (node.m_shape)
{
shapes.pushBack(node.m_shape);
node.m_shape->addReference();
}
for (int c = 0; c < node.m_childrenIdx.getSize(); c++)
{
hkpShape* childShape = buildShape(node.m_childrenIdx[c]);
if (childShape)
{
shapes.pushBack( childShape );
}
}
if (shapes.getSize())
{
hkpShape* combinedShape = new hkpListShape(shapes.begin(), shapes.getSize());
for (int s = 0; s < shapes.getSize(); s++)
{
shapes[s]->removeReference();
}
// all this under transform
hkpShape* transformed = new hkpTransformShape(combinedShape, node.m_transform);
combinedShape->removeReference();
return transformed;
}
return HK_NULL;
}
void ShapeState::init(QWidget* widget)
{
m_start = std::chrono::system_clock::now();
m_widget = widget;
StateData& stateData = getData();
m_last_target_tp = std::chrono::system_clock::now() - std::chrono::seconds(10);
std::cout << "Shape init " << std::endl;
if (m_data->iterationCount == 0)
{
//random generator shuffle
std::random_shuffle ( s_cells.begin(), s_cells.end() );
}
initParamsFromCell(s_cells[m_data->iterationCount]);
m_points = assignLTC(generatePoints(POINT_COUNT, RADIUS, m_params.sharpness), m_params.ltcr);
m_shape = buildShape(m_points);
}
void DestructibleHierarchy::breakOffNode(int nodeIdx, hkArray<DestructibleHierarchyCollisionEvent>& collisionEvents)
{
Node* node = &m_nodes[nodeIdx];
int immediateParent = node->m_parentIdx;
HK_ASSERT2(0xad78d9dd, immediateParent != INVALID_NODE_INDEX, "where's the parent?");
// detach from parent
Node* immediateParentNode = &m_nodes[immediateParent];
HK_ON_DEBUG( int initChildCount = immediateParentNode->m_childrenIdx.getSize() );
for (int c = 0; c < immediateParentNode->m_childrenIdx.getSize(); c++)
{
if (immediateParentNode->m_childrenIdx[c] == nodeIdx)
{
immediateParentNode->m_childrenIdx.removeAtAndCopy(c);
break;
}
}
HK_ASSERT2(0xad78ddaa, initChildCount-1 == immediateParentNode->m_childrenIdx.getSize(), "child not detached!" );
hkTransform accumulatedTransform = immediateParentNode->m_transform;
// Find parent rigid body..
int parentIdx = immediateParent;
Node* parentNode = immediateParentNode;
while(parentNode->m_body == HK_NULL)
{
parentIdx = parentNode->m_parentIdx;
parentNode = &m_nodes[parentIdx];
hkTransform tmp; tmp.setMul(parentNode->m_transform, accumulatedTransform);
accumulatedTransform = tmp;
HK_ASSERT2(0xad678daa, parentNode, "No parent rigid body found!");
}
// Create new body
hkpRigidBody* newBody;
{
hkpShape* shape = buildShape(nodeIdx);
hkpShape* flatShape = hkFlattenShapeHierarchyUtil::flattenHierarchy(shape);
shape->removeReference();
newBody = DestructibleHierarchy::buildRigidBody(flatShape, this, nodeIdx);
flatShape->removeReference();
const hkSmallArray< hkpCollisionListener* >& listeners = parentNode->m_body->getCollisionListeners();
for (int i = 0; i < listeners.getSize(); i++)
{
newBody->addCollisionListener(listeners[i]);
}
}
// init velocites for new body
{
// reposition the body
hkTransform parentTransform;
if (parentNode->m_initialTransformOfHierarchy)
{
parentTransform = *parentNode->m_initialTransformOfHierarchy;
}
else
{
parentTransform = parentNode->m_body->getTransform();
}
hkTransform newBodyTransform; newBodyTransform.setMul( parentTransform, accumulatedTransform );
newBody->setTransform(newBodyTransform);
// compute velocities
hkVector4 linVel = parentNode->m_body->getLinearVelocity();
hkVector4 angVel = parentNode->m_body->getAngularVelocity();
hkVector4 relCm; relCm.setSub4(newBody->getCenterOfMassInWorld(), parentNode->m_body->getCenterOfMassInWorld());
hkVector4 extraLin; extraLin.setCross(angVel, relCm);
linVel.add4(extraLin);
newBody->setLinearVelocity( linVel );
newBody->setAngularVelocity( angVel );
}
// set newBody position
parentNode->m_body->getWorld()->addEntity(newBody);
newBody->removeReference();
newBody = HK_NULL;
// Update shape of parent body
if (!parentNode->m_body->isFixed())
{
hkpShape* shape = buildShape(parentIdx);
if (shape)
{
hkVector4 oldCm = parentNode->m_body->getCenterOfMassInWorld();
hkpShape* flatShape = hkFlattenShapeHierarchyUtil::flattenHierarchy(shape);
updateShapeOfRigidBody(flatShape, parentNode->m_body);
shape->removeReference();
flatShape->removeReference();
hkVector4 relCm; relCm.setSub4(parentNode->m_body->getCenterOfMassInWorld(), oldCm);
hkVector4 extraLin; extraLin.setCross(parentNode->m_body->getAngularVelocity(), relCm);
hkVector4 linVel; linVel.setAdd4(parentNode->m_body->getLinearVelocity(), extraLin);
parentNode->m_body->setLinearVelocity(linVel);
}
else
{
parentNode->m_body->getWorld()->removeEntity(parentNode->m_body);
parentNode->m_body->removeReference();
parentNode->m_body = HK_NULL;
}
}
else // if (!parentNode->m_body->isFixed())
{
// if we're breaking off of a fixed shape -- this must be the one fixed mopp shape
const hkpShape* shape = parentNode->m_body->getCollidable()->getShape();
HK_ASSERT2(0xad1788dd, shape->getType() == HK_SHAPE_MOPP, "The fixed body must have a mopp.");
const hkpMoppBvTreeShape* mopp = static_cast<const hkpMoppBvTreeShape*>(shape);
// Remove shapeKeys from mopp
HK_ACCESS_CHECK_OBJECT(parentNode->m_body->getWorld(), HK_ACCESS_RW );
hkArray<hkpShapeKey> brokenOffShapeKeys;
collectShapeKeys(nodeIdx, brokenOffShapeKeys);
for (int i = brokenOffShapeKeys.getSize()-1; i >=0; i--)
{
if(brokenOffShapeKeys[i] == HK_INVALID_SHAPE_KEY)
{
brokenOffShapeKeys.removeAt(i);
}
else
{
const hkpMoppBvTreeShape* moppShape = static_cast<const hkpMoppBvTreeShape*>( parentNode->m_body->getCollidable()->getShape() );
removeSubShapeFromDisplay(parentNode->m_body, const_cast<hkpMoppBvTreeShape*>(moppShape), brokenOffShapeKeys[i]);
}
}
hkpRemoveTerminalsMoppModifier modifier(mopp->getMoppCode(), mopp->getShapeCollection(), brokenOffShapeKeys);
modifier.applyRemoveTerminals( const_cast<hkpMoppCode*>(mopp->getMoppCode()) );
// disable contact points for the removed shapes..
hkPointerMap<hkpShapeKey, int> shapeKeyMap;
shapeKeyMap.reserve(brokenOffShapeKeys.getSize());
for (int k = 0; k < brokenOffShapeKeys.getSize(); k++)
{
shapeKeyMap.insert(brokenOffShapeKeys[k], 0);
}
for (int e = 0; e < collisionEvents.getSize(); e++)
{
if (collisionEvents[e].m_contactMgr)
{
hkpShapeKey key = collisionEvents[e].m_shapeKey;
hkPointerMap<hkpShapeKey, int>::Iterator it = shapeKeyMap.findKey(key);
if (shapeKeyMap.isValid(it) && collisionEvents[e].m_body == parentNode->m_body)
{
static_cast<hkpDynamicsContactMgr*>(collisionEvents[e].m_contactMgr)->getContactPoint(collisionEvents[e].m_contactPointId)->setDistance(100000.0f);
collisionEvents.removeAt(e);
e--;
}
}
else
{
collisionEvents.removeAt(e);
e--;
}
}
}
}