void Leaf::fitBox(const mat3 &R, vec3 ¢er, vec3 &halfSize)
{
vec3 x = R.row(0);
vec3 y = R.row(1);
vec3 z = R.row(2);
vec3 max(-1.0e10, -1.0e10, -1.0e10);
vec3 min( 1.0e10, 1.0e10, 1.0e10);
std::list<Triangle>::iterator it;
for (it=mTriangles.begin(); it!=mTriangles.end(); it++) {
boxSize( (*it).v1, min, max, x, y, z, TOLERANCE);
boxSize( (*it).v2, min, max, x, y, z, TOLERANCE);
boxSize( (*it).v3, min, max, x, y, z, TOLERANCE);
}
DBGP("Max: " << max);
DBGP("Min: " << min);
for (int i=0; i<3; i++) {
halfSize[i] = 0.5 * (max[i] - min[i]);
}
DBGP("computed halfsize: " << halfSize);
//halfSize = 0.5 * (max - min);
center = min + halfSize;
center = R.inverse() * center;
//sanity check
for (int i=0; i<3; i++) {
if (halfSize[i] < TOLERANCE) {
if (halfSize[i] < 0.5 * TOLERANCE) {
DBGA("Warning: degenerate box computed");
}
halfSize[i] = TOLERANCE;
}
}
DBGP("returned halfsize: " << halfSize);
}
void GuiDecalEditorCtrl::renderScene(const RectI & updateRect)
{
PROFILE_SCOPE( GuiDecalEditorCtrl_renderScene );
GFXTransformSaver saver;
RectI bounds = getBounds();
ColorI hlColor(0,255,0,255);
ColorI regColor(255,0,0,255);
ColorI selColor(0,0,255,255);
ColorI color;
GFXDrawUtil *drawUtil = GFX->getDrawUtil();
GFXStateBlockDesc desc;
desc.setBlend( true );
desc.setZReadWrite( true, false );
// Draw 3D stuff here.
if ( mSELDecal )
{
mGizmo->renderGizmo( mLastCameraQuery.cameraMatrix, mLastCameraQuery.fov );
mSELEdgeVerts.clear();
if ( gDecalManager->clipDecal( mSELDecal, &mSELEdgeVerts ) )
_renderDecalEdge( mSELEdgeVerts, ColorI( 255, 255, 255, 255 ) );
const F32 &decalSize = mSELDecal->mSize;
Point3F boxSize( decalSize, decalSize, decalSize );
MatrixF worldMat( true );
mSELDecal->getWorldMatrix( &worldMat, true );
drawUtil->drawObjectBox( desc, boxSize, mSELDecal->mPosition, worldMat, ColorI( 255, 255, 255, 255 ) );
}
if ( mHLDecal )
{
mHLEdgeVerts.clear();
if ( gDecalManager->clipDecal( mHLDecal, &mHLEdgeVerts ) )
_renderDecalEdge( mHLEdgeVerts, ColorI( 255, 255, 255, 255 ) );
const F32 &decalSize = mHLDecal->mSize;
Point3F boxSize( decalSize, decalSize, decalSize );
MatrixF worldMat( true );
mHLDecal->getWorldMatrix( &worldMat, true );
drawUtil->drawObjectBox( desc, boxSize, mHLDecal->mPosition, worldMat, ColorI( 255, 255, 255, 255 ) );
}
}
void GameLayer::importGroundData(cocos2d::TMXTiledMap* data)
{
TMXObjectGroup *objects = m_pTiledMap->getObjectGroup("Grounds");
const ValueVector _objects = objects->getObjects();
if (!_objects.empty())
{
for (const auto& v : _objects)
{
const ValueMap& dict = v.asValueMap();
if (dict.find("name") != dict.end())
{
if (dict.at("name").asString() == "Ground" || dict.at("name").asString() == "Box")
{
Size boxSize(dict.at("width").asFloat(), dict.at("height").asFloat());
auto ground = Ground::create();
if(dict.find("rotation") != dict.end())
ground->initPhysics(boxSize, Point(dict.at("x").asFloat(), dict.at("y").asFloat()), dict.at("rotation").asInt());
else
ground->initPhysics(boxSize, Point(dict.at("x").asFloat(), dict.at("y").asFloat()), 0);
this->addChild(ground);
}
}
}
}
}
std::string Context::describe() {
namespace rus = readdy::util::str;
configure();
std::string description;
description += fmt::format("Configured kernel context with:{}", rus::newline);
description += fmt::format("--------------------------------{}", rus::newline);
description += fmt::format(" - kBT = {}{}", kBT(), rus::newline);
description += fmt::format(" - periodic b.c. = ({}, {}, {}){}",
periodicBoundaryConditions()[0],
periodicBoundaryConditions()[1],
periodicBoundaryConditions()[2],
rus::newline);
description += fmt::format(" - box size = ({}, {}, {}){}", boxSize()[0], boxSize()[1], boxSize()[2], rus::newline);
description += _particleTypeRegistry.describe();
description += _potentialRegistry.describe();
description += _reactionRegistry.describe();
description += _topologyRegistry.describe();
return description;
}
/// Main worker function (entry method).
/// In this simple example, an element performs the following:
/// 1. Create a bunch of boxes from this element. These are all
/// in the x-y plane and all parallel to the y-axis.
/// 2. Stretch the first box over into next object.
/// With this, the first box on elment 'e' will overlap with the first
/// and second box on element 'e+1'
/// 3. Send these objects to be collided. Results go to the collide manager
/// group with which this chunk (element) was registered at construction.
/// This is done by invoking the method 'contribute' on the local branch
/// of the collide manager group, passing it the chunk (element) number,
/// the number of boxes created by this element, the actual boxes, and
/// the array of priority flags.
///
/// Setting priority=NULL here will use the default setting which is
/// priority=thisIndex. In other words, objects created in the same
/// chunk (i.e. by the same element) will not collide with each other.
///
/// \see CollideBoxesPrio defined in collidecharm.{h,C}
///
void DoIt(void)
{
CkPrintf("Contributing to reduction %d, element %04d\n", nTimes, thisIndex);
// Create a bunch of boxes from this element.
////CkVector3d o(-6.8, 7.9, 8.0);
CkVector3d o( 0, 0, 0);
CkVector3d x( 4, 0, 0);
CkVector3d y( 0, 0.3, 0);
CkVector3d boxSize(0.2, 0.2, 0.2);
////int nBoxes = 1000;
int nBoxes = 10;
bbox3d *box = new bbox3d[nBoxes];
for (int i=0; i<nBoxes; i++) {
CkVector3d c(o + x*thisIndex + y*i);
CkVector3d c2(c + boxSize);
// Create a box with corners 'c' and 'c2'
box[i].empty();
box[i].add(c); box[i].add(c2);
#ifdef RADU_DBG
// Write box information
rSeg1d x_dim = box[i].axis(0);
rSeg1d y_dim = box[i].axis(1);
rSeg1d z_dim = box[i].axis(2);
CkPrintf("%d:%d (%g, %g, %g) - (%g, %g, %g)\n",
thisIndex, i,
x_dim.getMin(), y_dim.getMin(), z_dim.getMin(),
x_dim.getMax(), y_dim.getMax(), z_dim.getMax());
#endif
}
// Stretch the first box over into next object.
box[0].add(o + x*(thisIndex+1.5) + y*2);
#ifdef RADU_DBG
// Stretched box[0]
rSeg1d x_dim = box[0].axis(0);
rSeg1d y_dim = box[0].axis(1);
rSeg1d z_dim = box[0].axis(2);
CkPrintf(">>> %d:0 (%g, %g, %g) - (%g, %g, %g)\n",
thisIndex,
x_dim.getMin(), y_dim.getMin(), z_dim.getMin(),
x_dim.getMax(), y_dim.getMax(), z_dim.getMax());
#endif
// Send these objects to be collided.
CollideBoxesPrio(collide, thisIndex, nBoxes, box, NULL);
delete[] box;
nTimes++;
}
void BenchmarkDemo::createTest6()
{
setCameraDistance(btScalar(250.));
btVector3 boxSize(1.5f,1.5f,1.5f);
btConvexHullShape* convexHullShape = new btConvexHullShape();
for (int i=0;i<TaruVtxCount;i++)
{
btVector3 vtx(TaruVtx[i*3],TaruVtx[i*3+1],TaruVtx[i*3+2]);
convexHullShape->addPoint(vtx);
}
btTransform trans;
trans.setIdentity();
float mass = 1.f;
btVector3 localInertia(0,0,0);
convexHullShape->calculateLocalInertia(mass,localInertia);
{
int size = 10;
int height = 10;
const float cubeSize = boxSize[0];
float spacing = 2.0f;
btVector3 pos(0.0f, 20.0f, 0.0f);
float offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
for(int k=0;k<height;k++) {
for(int j=0;j<size;j++) {
pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
for(int i=0;i<size;i++) {
pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
btVector3 bpos = btVector3(0,25,0) + btVector3(5.0f,1.0f,5.0f)*pos;
trans.setOrigin(bpos);
localCreateRigidBody(mass,trans,convexHullShape);
}
}
offset -= 0.05f * spacing * (size-1);
spacing *= 1.1f;
pos[1] += (cubeSize * 2.0f + spacing);
}
}
createLargeMeshBody();
}
/*! The split is done along the given x axis, and the optimal child bounding boxes
are aligned with the x,y,z axes. Of course, there is no guarantee that the
children will actually build their own bboxes along those axes; as they will
do their own principal components they might end up with boxes aligned
differently.
*/
void
Leaf::optimalSplit(const vec3 &x, const vec3 &y, const vec3 &z, Leaf *child1, Leaf *child2)
{
//sort triangles by centroid projections
std::vector< std::pair<Triangle,double> > sortedTriangles;
std::list<Triangle>::iterator it;
for (it=mTriangles.begin(); it!=mTriangles.end(); it++) {
position median = (*it).v1 + (*it).v2 + (*it).v3;
median = ( 1.0 / 3.0 ) * median;
double projection = (median - position::ORIGIN) % x;
sortedTriangles.push_back( std::pair<Triangle,double>(*it,projection) );
}
std::sort(sortedTriangles.begin(), sortedTriangles.end(), compareProjections);
//compute bbox volumes going up
std::vector<double> volumesUp;
vec3 max(-1.0e10, -1.0e10, -1.0e10);
vec3 min( 1.0e10, 1.0e10, 1.0e10);
std::vector< std::pair<Triangle,double> >::iterator it2;
for (it2 = sortedTriangles.begin(); it2!=sortedTriangles.end(); it2++) {
boxSize((*it2).first.v1, min, max, x, y, z, Leaf::TOLERANCE);
boxSize((*it2).first.v2, min, max, x, y, z, Leaf::TOLERANCE);
boxSize((*it2).first.v3, min, max, x, y, z, Leaf::TOLERANCE);
double volumeSq = (max[0] - min[0]) * (max[1] - min[1]) * (max[2] - min[2]);
volumesUp.push_back(volumeSq);
}
//and bbox volumes going down
std::vector<double> volumesDown;
max.set(-1.0e10, -1.0e10, -1.0e10);
min.set( 1.0e10, 1.0e10, 1.0e10);
std::vector< std::pair<Triangle,double> >::reverse_iterator it3;
for (it3 = sortedTriangles.rbegin(); it3!=sortedTriangles.rend(); it3++) {
boxSize((*it3).first.v1, min, max, x, y, z, Leaf::TOLERANCE);
boxSize((*it3).first.v2, min, max, x, y, z, Leaf::TOLERANCE);
boxSize((*it3).first.v3, min, max, x, y, z, Leaf::TOLERANCE);
double volumeSq = (max[0] - min[0]) * (max[1] - min[1]) * (max[2] - min[2]);
volumesDown.push_back(volumeSq);
}
assert( volumesUp.size() == volumesDown.size() );
//find optimal split point
for (int i=0; i<(int)volumesUp.size(); i++) {
volumesUp[i] += volumesDown[ volumesDown.size()-i-1 ];
}
std::vector<double>::iterator minVolIt = std::min_element(volumesUp.begin(), volumesUp.end());
//assign triangles to children
std::vector<double>::iterator it4;
int index=0;
for (it4 = volumesUp.begin(); it4 != minVolIt; it4++) {
child1->addTriangle(sortedTriangles[index].first);
index++;
}
for (it4 = minVolIt; it4 != volumesUp.end(); it4++) {
child2->addTriangle(sortedTriangles[index].first);
index++;
}
}
/// get the intersected piece from a ray in world space
int cRubikSnake::GetRayIntersection( TSRVector3& raySource, TSRVector3& rayDirection )
{
float Dist = FLT_MAX;
float newDist = 0.0f;
TSRMatrix4 currTransform;
currTransform.MakeIdent();
int iPossibleMoveIndex = -1;
float dummyT;
TSRVector3 q;
TSRMatrixStack stack;
stack.LoadIdentity();
stack.Push();
stack.TranslateLocal( m_vPanTranslation.x, m_vPanTranslation.y, m_vPanTranslation.z );
stack.MultMatrix( m_ArcBall.m_Transform.d );
stack.TranslateLocal( -m_Center.x, -m_Center.y, -m_Center.z );
TSRVector3 rotAxis( 1.0f, 0.0f, 0.0f );
for ( unsigned int i = 0; i < m_Angles.size(); i++ )
{
stack.Push();
stack.TranslateLocal( 0.25f, 0.25f, 0.0f );
memcpy( currTransform.d, stack.GetTop(), sizeof( TSRMatrix4 ) );
stack.Pop();
TSRVector3 boxSize( 0.6f, 0.6f, 0.6f );
stack.MultMatrix( m_Transform.d );
if ( TSRCollisionTests::IntersectRayOBB( raySource, rayDirection, currTransform, boxSize, dummyT, q ) )
{
newDist = ( raySource - q ).Mag();
if ( newDist < Dist )
{
Dist = newDist;
iPossibleMoveIndex = i;
m_SelectedIndex = i;
}
}
stack.TranslateLocal( -0.5f, 0.5f, 0.0f );
stack.RotateAxisLocal( rotAxis, m_Angles[ i ] * PI / 180.0f );
}
stack.Pop();
return iPossibleMoveIndex;
}
void GridDisplay::drawGrid() {
int width = round((double)this->matSize.width/this->gridSize.width);
int height = round((double)this->matSize.height/this->gridSize.height);
Size boxSize(width,height);
Point endPt;
int row=0, col=0;
while(row<this->img.rows) {
while(col<this->img.cols) {
endPt.x = min(col+boxSize.width-1,img.cols-1);
endPt.y = min(row+boxSize.height-1,img.rows-1);
rectangle(this->img,Point(col,row),endPt,Scalar(100));
col = endPt.x+1;
}
col=0;
row = endPt.y+1;
}
}
int main(int argc, char **argv){
int peid, numpes;
MPI_Comm newComm;
//basic MPI initilization
MPI_Init(&argc, &argv);
MPI_Comm_rank(MPI_COMM_WORLD, &peid);
MPI_Comm_size(MPI_COMM_WORLD, &numpes);
//initialize Charm for each set
CharmLibInit(MPI_COMM_WORLD, argc, argv);
MPI_Barrier(MPI_COMM_WORLD);
CollisionList *colls;
CkVector3d o(-6.8,7.9,8.0), x(4.0,0,0), y(0,0.3,0);
CkVector3d boxSize(0.2,0.2,0.2);
int nBoxes=1000;
bbox3d *box=new bbox3d[nBoxes];
for (int i=0;i<nBoxes;i++) {
CkVector3d c(o+x*peid+y*i);
CkVector3d c2(c+boxSize);
box[i].empty();
box[i].add(c); box[i].add(c2);
}
// first box stretches over into next object:
box[0].add(o+x*(peid+1.5)+y*2);
detectCollision(colls,nBoxes, box, NULL);
int numColls=colls->length();
for (int c=0;c<numColls;c++) {
printf("%d:%d hits %d:%d\n",
(*colls)[c].A.chunk,(*colls)[c].A.number,
(*colls)[c].B.chunk,(*colls)[c].B.number);
}
delete box;
MPI_Barrier(MPI_COMM_WORLD);
CharmLibExit();
//final synchronization
MPI_Barrier(MPI_COMM_WORLD);
MPI_Finalize();
return 0;
}
void createSceneJoints()
{
const int n = 10;
int startId = numRigidBodies;
PfxVector3 boxSize(1.0f);
PfxFloat boxMass = 1.0f;
for(int i=0;i<n;i++) {
createBrick(numRigidBodies++,PfxVector3(0,3.0f+i*2.5f*boxSize[1],0),PfxQuat::identity(),boxSize,boxMass);
}
for(int i=startId;i<startId+n;i++) {
PfxRigidState &stateA = states[i];
PfxRigidState &stateB = states[(i+1)%numRigidBodies];
PfxVector3 anchor;
if(i == numRigidBodies-1) {
anchor = stateA.getPosition() + PfxVector3(0,boxSize[1],0);
}
else {
anchor = ( stateA.getPosition() + stateB.getPosition() ) * 0.5f;
}
PfxSwingTwistJointInitParam jparam;
jparam.anchorPoint = anchor;
jparam.twistAxis = PfxVector3(0,1,0);
pfxInitializeSwingTwistJoint(joints[numJoints],stateA,stateB,jparam);
joints[numJoints].m_constraints[4].m_damping = 0.1f;
joints[numJoints].m_constraints[5].m_damping = 0.1f;
SCE_PFX_ASSERT(numJoints<NUM_JOINTS);
numJoints++;
}
states[startId].setLinearVelocity(PfxVector3(0,0,5));
states[startId].setLinearDamping(0.95f);
states[startId].setAngularDamping(0.95f);
}
GLFont::Box GLFont::calcStringBox(GLsizei stringWidth) const
{
/* Calculate the string's scaled width: */
Vector boxSize(GLfloat(stringWidth-1)*textHeight/GLfloat(fontHeight-1),textHeight,0.0f);
/* Calculate the string's bounding box origin: */
Vector boxOrigin(0.0f,0.0f,0.0f);
switch(hAlignment)
{
case Left:
boxOrigin[0]=0.0f;
break;
case Center:
boxOrigin[0]=-0.5f*boxSize[0];
break;
case Right:
boxOrigin[0]=-boxSize[0];
break;
}
switch(vAlignment)
{
case Top:
boxOrigin[1]=-boxSize[1];
break;
case VCenter:
boxOrigin[1]=-0.5f*boxSize[1];
break;
case Baseline:
boxOrigin[1]=-boxSize[1]*GLfloat(baseLine)/GLfloat(fontHeight);
break;
case Bottom:
boxOrigin[1]=0.0f;
break;
}
return Box(boxOrigin,boxSize);
}
QRectF PipelineFlowChart::boxRect(int i)
{
QRectF totalRect = totalAreaRect();
qreal boxeswidth = totalRect.width() - numGaps() * boxMargin();
qreal boxdim = qMin(totalRect.height(), boxeswidth / numItems());
boxdim = qMax(MinBoxDimension, boxdim);
qreal oblongwidth = qMax(0.0, (boxeswidth - boxdim * numItems()) / numItems());
QSizeF boxSize(boxdim + oblongwidth, boxdim);
QRectF ret(totalRect.x() + i * (boxSize.width() + boxMargin()),
totalRect.y() + totalRect.height() / 2 - boxSize.height() / 2, boxSize.width(),
boxSize.height());
return ret;
}
void DemoKeeper::BrickWallDemo( void )
{
mWorld->markForWrite();
//
// Create the walls
//
hkVector4 groundPos( 0.0f, 0.0f, 0.0f );
hkVector4 boxSize( 1.0f, 1.0f, 1.0f);
hkReal deltaZ = 25.0f;
groundPos(2) = -deltaZ * 10 * 0.5f;
for ( int y = 0; y < 4; y ++ ) // first wall
{
createBrickWall( 4, 8, groundPos, 0.2f, boxSize );
groundPos(2) += deltaZ;
}
mWorld->unmarkForWrite();
}
void DoIt(void)
{
CkPrintf("Contributing to reduction %d, element %04d\n",nTimes,thisIndex);
CkVector3d o(-6.8,7.9,8.0), x(4.0,0,0), y(0,0.3,0);
CkVector3d boxSize(0.2,0.2,0.2);
int nBoxes=1000;
bbox3d *box=new bbox3d[nBoxes];
for (int i=0;i<nBoxes;i++) {
CkVector3d c(o+x*thisIndex+y*i);
CkVector3d c2(c+boxSize);
box[i].empty();
box[i].add(c); box[i].add(c2);
}
// first box stretches over into next object:
box[0].add(o+x*(thisIndex+1.5)+y*2);
CollideBoxesPrio(collide,thisIndex,nBoxes,box,NULL);
delete[] box;
nTimes++;
}
void Player::update(float dt)
{
Rect rect = m_model->realBoundingBoxForCurrentFrame();
Vec3 scale, pos; Quaternion rot;
getNodeToWorldTransform().decompose(&scale, &rot, &pos);
do
{
Node* level = Director::getInstance()->getRunningScene()->getChildByTag(1);
if (level == nullptr)
{
break;
}
Vec2 position = level->getPosition();
if (m_state == EWalkLeft)
{
position += dt * m_speed * Vec2(scale.x, 0);
}
else if (m_state == EWalkRight)
{
position += dt * m_speed * Vec2(-scale.x, 0);
}
else
{
break;
}
level->setPosition(position);
} while (0);
Size boxSize(rect.size.width * scale.x, rect.size.height * scale.y);
Vec2 boxPos(rect.origin.x * scale.x + boxSize.width / 2, rect.origin.y * scale.y + boxSize.height / 2);
auto body = PhysicsBody::createBox(boxSize, PHYSICSBODY_MATERIAL_DEFAULT);
body->setPositionOffset(boxPos);
setPhysicsBody(body);
body->setContactTestBitmask(0x2);
}
void Box::apply_plane_collision(float time, Map* ptr) {
collision = false;
sf::Vector2f boxSize(rectW,rectL);
sf::Vector2f mapSize(800,5);
sf::Vector2f planePos = ptr->getPlanePosition();
sf::Vector2f boxPos = rect.getPosition();
sf::FloatRect planeRec(planePos, mapSize );
sf::FloatRect boxRec(boxPos,boxSize);
sf::Vector2f distance = planePos - boxPos;
float dist = sqrt(pow(distance.x,2) + pow(distance.y,2) );
if( dist < 2*rectW) {
if( boxRec.intersects(planeRec) ){
if(!collision) {
collision = true;
bounceNumber++;
speed = -speed;
}
}
}
}
hkpRigidBody* PairCollisionFilter::createMovingBox() {
hkVector4 boxSize(.5f, .5f, .5f);
hkpShape* boxShape = new hkpBoxShape(boxSize, 0);
hkpRigidBodyCinfo info;
info.m_shape = boxShape;
info.m_qualityType = HK_COLLIDABLE_QUALITY_MOVING;
hkReal boxMass = 1.f;
hkMassProperties massProperties;
hkpInertiaTensorComputer::computeBoxVolumeMassProperties(boxSize, boxMass, massProperties);
info.m_motionType = hkpMotion::MOTION_BOX_INERTIA;
info.m_mass = boxMass;
info.m_inertiaTensor = massProperties.m_inertiaTensor;
info.m_position.set(0.f, 7.f, 0.f);
hkpRigidBody* boxBody = new hkpRigidBody(info);
m_world->addEntity(boxBody);
boxBody->removeReference();
boxShape->removeReference();
return boxBody;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale)
{
dgAssert(proxy.m_referenceCollision->IsType(dgCollision::dgCollisionConvexShape_RTTI));
dgAssert(proxy.m_floatingCollision->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
const dgCollisionInstance* const polygonInstance = proxy.m_floatingCollision;
dgAssert(this == polygonInstance->GetChildShape());
dgAssert(m_count);
dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));
dgInt32 count = 0;
m_normal = m_normal.CompProduct4(polyInstanceInvScale);
dgAssert(m_normal.m_w == dgFloat32(0.0f));
m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt());
dgVector savedFaceNormal(m_normal);
dgVector savedPosit (proxy.m_matrix.m_posit);
proxy.m_matrix.m_posit = dgVector::m_wOne;
dgVector hullOrigin(proxy.m_matrix.UnrotateVector (savedPosit));
for (dgInt32 i = 0; i < m_count; i++) {
m_localPoly[i] = hullOrigin + polyInstanceScale.CompProduct4(dgVector(&m_vertex[m_vertexIndex[i] * m_stride]));
dgAssert(m_localPoly[i].m_w == dgFloat32(0.0f));
}
dgContact* const contactJoint = proxy.m_contactJoint;
const dgCollisionInstance* const hull = proxy.m_referenceCollision;
dgVector normalInHull(proxy.m_matrix.RotateVector(m_normal));
dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL));
dgVector p0(proxy.m_matrix.UntransformVector(pointInHull));
dgVector p1(proxy.m_matrix.UntransformVector(hull->SupportVertex(normalInHull, NULL)));
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32(0.0f)) {
contactJoint->m_closestDistance = -penetration;
proxy.m_matrix.m_posit = savedPosit;
return 0;
}
contactJoint->m_closestDistance = dgFloat32(0.0f);
dgFloat32 distance = (m_localPoly[0] - p1) % m_normal;
if (distance >= dgFloat32(0.0f)) {
proxy.m_matrix.m_posit = savedPosit;
return 0;
}
dgVector boxSize (hull->GetBoxSize() & dgVector::m_triplexMask);
dgVector boxOrigin ((hull->GetBoxOrigin() & dgVector::m_triplexMask) + dgVector::m_wOne);
bool inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i++) {
dgVector e(m_localPoly[i] - m_localPoly[i0]);
dgVector n(m_normal * e);
//dgPlane plane(n, -(m_localPoly[i0] % n));
dgPlane plane(n, - m_localPoly[i0].DotProduct4 (n).GetScalar());
plane = proxy.m_matrix.TransformPlane(plane);
//dgFloat32 supportDist = dgAbsf(plane.m_x) * boxSize.m_x + dgAbsf(plane.m_y) * boxSize.m_y + dgAbsf(plane.m_z) * boxSize.m_z;
//dgFloat32 centerDist = plane.Evalue(boxOrigin);
dgFloat32 supportDist = boxSize.DotProduct4 (plane.Abs()).GetScalar();
dgFloat32 centerDist = plane.DotProduct4 (boxOrigin).GetScalar();
if ((centerDist + supportDist) < dgFloat32(0.0f)) {
proxy.m_matrix.m_posit = savedPosit;
return 0;
}
if ((centerDist - supportDist) < dgFloat32(0.0f)) {
inside = false;
break;
}
i0 = i;
}
const dgInt32 hullId = hull->GetUserDataID();
if (inside & !proxy.m_intersectionTestOnly) {
dgAssert(penetration >= dgFloat32(0.0f));
dgVector pointsContacts[64];
dgAssert(penetration >= 0.0f);
dgVector point(pointInHull + normalInHull.Scale4(penetration));
count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, pointsContacts, 1.0f);
dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f)));
const dgMatrix& worldMatrix = hull->m_globalMatrix;
dgContactPoint* const contactsOut = proxy.m_contacts;
dgAssert(contactsOut);
dgVector globalNormal(worldMatrix.RotateVector(normalInHull));
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_point = worldMatrix.TransformVector(pointsContacts[i] + step);
contactsOut[i].m_normal = globalNormal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
} else {
dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize();
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping(dgVector(dgFloat32(0.0f)), convexSphapeUmbra);
m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
}
dgCollisionConvex* const convexShape = (dgCollisionConvex*)hull->m_childShape;
count = convexShape->CalculateConvexToConvexContact(proxy);
dgAssert(proxy.m_intersectionTestOnly || (count >= 0));
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
if (m_closestFeatureType == 3) {
for (dgInt32 i = 0; i < count; i++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
dgVector normal(polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal));
if (normal.DotProduct4(savedFaceNormal).GetScalar() < dgFloat32(0.9995f)) {
dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
dgVector n(&m_vertex[index * m_stride]);
if ((savedFaceNormal.DotProduct4(n).GetScalar() > dgFloat32(0.9995f))) {
normal = n;
} else {
dgVector dir0(n * savedFaceNormal);
dgVector dir1(n * normal);
dgFloat32 projection = dir0.DotProduct4(dir1).GetScalar();
if (projection <= dgFloat32(0.0f)) {
normal = n;
}
}
normal = polygonInstance->m_globalMatrix.RotateVector(normal);
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_normal = normal;
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
for (dgInt32 i = 0; i < count; i++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
}
proxy.m_matrix.m_posit = savedPosit;
return count;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue (dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale)
{
dgAssert (proxy.m_referenceCollision->IsType (dgCollision::dgCollisionConvexShape_RTTI));
dgAssert (proxy.m_floatingCollision->IsType (dgCollision::dgCollisionConvexPolygon_RTTI));
const dgCollisionInstance* const hull = proxy.m_referenceCollision;
dgAssert (this == proxy.m_floatingCollision->GetChildShape());
dgAssert (m_count);
dgAssert (m_count < dgInt32 (sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgBody* const floatingBody = proxy.m_floatingBody;
const dgBody* const referenceBody = proxy.m_referenceBody;
dgContact* const contactJoint = proxy.m_contactJoint;
contactJoint->m_closestDistance = dgFloat32 (1.0e10f);
m_normal = m_normal.CompProduct4(polyInstanceInvScale);
dgAssert (m_normal.m_w == dgFloat32 (0.0f));
m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt());
const dgVector savedFaceNormal (m_normal);
for (dgInt32 i = 0; i < m_count; i ++) {
m_localPoly[i] = polyInstanceScale.CompProduct4(dgVector (&m_vertex[m_vertexIndex[i] * m_stride]));
dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
}
dgVector hullOrigin (proxy.m_matrix.UntransformVector(dgVector (dgFloat32 (0.0f))));
hullOrigin = (hullOrigin - m_normal.CompProduct4(m_normal.DotProduct4(hullOrigin - m_localPoly[0]))) | dgVector::m_wOne;
dgMatrix polygonMatrix;
polygonMatrix[0] = m_localPoly[1] - m_localPoly[0];
polygonMatrix[0] = polygonMatrix[0].CompProduct4 (polygonMatrix[0].InvMagSqrt());
polygonMatrix[1] = m_normal;
polygonMatrix[2] = polygonMatrix[0] * m_normal;
polygonMatrix[3] = hullOrigin;
dgAssert (polygonMatrix.TestOrthogonal());
dgMatrix savedProxyMatrix (proxy.m_matrix);
proxy.m_matrix = polygonMatrix * proxy.m_matrix;
dgVector floatingVeloc (floatingBody->m_veloc);
dgVector referenceVeloc (referenceBody->m_veloc);
const dgMatrix& hullMatrix = hull->GetGlobalMatrix();
dgVector hullRelativeVeloc (hullMatrix.UnrotateVector(referenceVeloc - floatingVeloc));
dgVector polyRelativeVeloc (proxy.m_matrix.UnrotateVector (hullRelativeVeloc));
dgVector polyBoxP0 (dgFloat32 ( 1.0e15f));
dgVector polyBoxP1 (dgFloat32 (-1.0e15f));
m_normal = polygonMatrix.UnrotateVector(m_normal);
if (m_normal.DotProduct4(polyRelativeVeloc).m_x >= 0.0f) {
proxy.m_matrix = savedProxyMatrix;
return 0;
}
for (dgInt32 i = 0; i < m_count; i ++) {
m_localPoly[i] = polygonMatrix.UntransformVector(m_localPoly[i]);
dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
polyBoxP0 = polyBoxP0.GetMin (m_localPoly[i]);
polyBoxP1 = polyBoxP1.GetMax (m_localPoly[i]);
}
dgInt32 count = 0;
dgVector hullBoxP0;
dgVector hullBoxP1;
hull->CalcAABB (proxy.m_matrix.Inverse(), hullBoxP0, hullBoxP1);
dgVector minBox (polyBoxP0 - hullBoxP1);
dgVector maxBox (polyBoxP1 - hullBoxP0);
dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), polyRelativeVeloc);
dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);
if (distance < dgFloat32 (1.0f)) {
dgVector boxSize ((hullBoxP1 - hullBoxP0).Scale4 (dgFloat32 (0.5f)));
// dgVector boxOrigin ((hullBoxP1 + hullBoxP0).Scale4 (dgFloat32 (0.5f)));
// boxOrigin += polyRelativeVeloc.Scale4 (distance);
dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
dgVector pointInPlane (proxy.m_matrix.UntransformVector (pointInHull));
dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;
dgFloat32 timeToPlane = distToPlane / (polyRelativeVeloc % m_normal);
dgVector boxOrigin (pointInPlane + polyRelativeVeloc.Scale4(timeToPlane));
bool inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i ++) {
dgVector e (m_localPoly[i] - m_localPoly[i0]);
dgVector n (m_normal * e);
dgPlane plane (n, - (m_localPoly[i0] % n));
dgVector supportDist (plane.Abs().DotProduct4 (boxSize));
dgFloat32 centerDist = plane.Evalue(boxOrigin);
if ((centerDist + supportDist.m_x) < dgFloat32 (0.0f)) {
proxy.m_matrix = savedProxyMatrix;
return 0;
}
if ((centerDist - supportDist.m_x) < dgFloat32 (0.0f)) {
inside = false;
}
i0 = i;
}
// for the time being for the minkousky contact calculation
inside = false;
const dgInt32 hullId = hull->GetUserDataID();
if (inside) {
dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
dgVector p0 (proxy.m_matrix.UntransformVector (pointInHull));
dgFloat32 timetoImpact = dgFloat32 (0.0f);
//dgFloat32 closestDistance = dgFloat32 (0.0f);
dgAssert (0);
// dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness + DG_IMPULSIVE_CONTACT_PENETRATION;
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32 (0.0f)) {
timetoImpact = penetration / (polyRelativeVeloc % m_normal);
dgAssert (timetoImpact >= dgFloat32 (0.0f));
// closestDistance = -penetration;
}
if (timetoImpact <= proxy.m_timestep) {
dgVector pointsContacts[64];
contactJoint->m_closestDistance = penetration;
dgAssert (0);
// dgVector point (pointInHull - normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION));
dgVector point (pointInHull);
count = hull->CalculatePlaneIntersection (normalInHull, point, pointsContacts, 1.0f);
dgAssert (0);
// dgVector step (hullRelativeVeloc.Scale3 (timetoImpact) + normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION));
dgVector step (hullRelativeVeloc.Scale3 (timetoImpact));
penetration = dgMax (penetration, dgFloat32 (0.0f));
const dgMatrix& worldMatrix = hull->m_globalMatrix;
dgContactPoint* const contactsOut = proxy.m_contacts;
dgVector globalNormal (worldMatrix.RotateVector(normalInHull));
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_point = worldMatrix.TransformVector (pointsContacts[i] + step);
contactsOut[i].m_normal = globalNormal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
}
} else {
dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize ();
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping (boxOrigin, convexSphapeUmbra);
m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
}
dgCollisionConvex* const convexShape = (dgCollisionConvex*) hull->m_childShape;
count = convexShape->CalculateConvexCastContacts (proxy);
// dgAssert (proxy.m_intersectionTestOnly || (count >= 0));
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
#if 0
if (m_closestFeatureType == 3) {
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
dgVector normal (polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal));
if ((normal % savedFaceNormal) < dgFloat32 (0.995f)) {
dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
dgVector n (&m_vertex[index * m_stride]);
dgVector dir0 (n * savedFaceNormal);
dgVector dir1 (n * normal);
dgFloat32 projection = dir0 % dir1;
if (projection <= dgFloat32 (0.0f)) {
normal = n;
}
normal = polygonInstance->m_globalMatrix.RotateVector(normal);
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_normal = normal;
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
for (dgInt32 i = 0; i < count; i ++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
#endif
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
proxy.m_matrix = savedProxyMatrix;
return count;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
dgAssert(this == proxy.m_instance1->GetChildShape());
dgAssert(m_count);
dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgBody* const body0 = proxy.m_body0;
const dgBody* const body1 = proxy.m_body1;
dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());
dgVector relativeVelocity (body0->m_veloc - body1->m_veloc);
if (m_normal.DotProduct4(relativeVelocity).GetScalar() >= 0.0f) {
return 0;
}
dgFloat32 den = dgFloat32 (1.0f) / (relativeVelocity % m_normal);
if (den > dgFloat32 (1.0e-5f)) {
// this can actually happens
dgAssert(0);
return 0;
}
dgContact* const contactJoint = proxy.m_contactJoint;
contactJoint->m_closestDistance = dgFloat32(1.0e10f);
dgMatrix polygonMatrix;
dgVector right (m_localPoly[1] - m_localPoly[0]);
polygonMatrix[0] = right.CompProduct4(right.InvMagSqrt());
polygonMatrix[1] = m_normal;
polygonMatrix[2] = polygonMatrix[0] * m_normal;
polygonMatrix[3] = dgVector::m_wOne;
dgAssert (polygonMatrix.TestOrthogonal());
dgVector polyBoxP0(dgFloat32(1.0e15f));
dgVector polyBoxP1(dgFloat32(-1.0e15f));
for (dgInt32 i = 0; i < m_count; i++) {
dgVector point (polygonMatrix.UnrotateVector(m_localPoly[i]));
polyBoxP0 = polyBoxP0.GetMin(point);
polyBoxP1 = polyBoxP1.GetMax(point);
}
dgVector hullBoxP0;
dgVector hullBoxP1;
dgMatrix hullMatrix (polygonMatrix * proxy.m_instance0->m_globalMatrix);
proxy.m_instance0->CalcAABB(hullMatrix, hullBoxP0, hullBoxP1);
dgVector minBox(polyBoxP0 - hullBoxP1);
dgVector maxBox(polyBoxP1 - hullBoxP0);
dgVector veloc (polygonMatrix.UnrotateVector (relativeVelocity));
dgFastRayTest ray(dgVector(dgFloat32(0.0f)), veloc);
dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);
dgInt32 count = 0;
if (distance < dgFloat32(1.0f)) {
bool inside = false;
dgVector boxSize((hullBoxP1 - hullBoxP0).CompProduct4(dgVector::m_half));
dgVector sphereMag2 (boxSize.DotProduct4(boxSize));
boxSize = sphereMag2.Sqrt();
dgVector pointInPlane (polygonMatrix.RotateVector(hullBoxP1 + hullBoxP0).CompProduct4(dgVector::m_half));
dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;
dgFloat32 timeToPlane0 = (distToPlane + boxSize.GetScalar()) * den;
dgFloat32 timeToPlane1 = (distToPlane - boxSize.GetScalar()) * den;
dgVector boxOrigin0 (pointInPlane + relativeVelocity.Scale4(timeToPlane0));
dgVector boxOrigin1 (pointInPlane + relativeVelocity.Scale4(timeToPlane1));
dgVector boxOrigin ((boxOrigin0 + boxOrigin1).CompProduct4(dgVector::m_half));
dgVector boxProjectSize (((boxOrigin0 - boxOrigin1).CompProduct4(dgVector::m_half)));
sphereMag2 = boxProjectSize.DotProduct4(boxProjectSize);
boxSize = sphereMag2.Sqrt();
dgAssert (boxOrigin.m_w == 0.0f);
boxOrigin = boxOrigin | dgVector::m_wOne;
if (!proxy.m_intersectionTestOnly) {
inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i++) {
dgVector e(m_localPoly[i] - m_localPoly[i0]);
dgVector n(m_normal * e & dgVector::m_triplexMask);
dgFloat32 param = dgSqrt (sphereMag2.GetScalar() / (n.DotProduct4(n)).GetScalar());
dgPlane plane(n, -(m_localPoly[i0] % n));
dgVector p0 (boxOrigin + n.Scale4 (param));
dgVector p1 (boxOrigin - n.Scale4 (param));
dgFloat32 size0 = (plane.DotProduct4 (p0)).GetScalar();
dgFloat32 size1 = (plane.DotProduct4 (p1)).GetScalar();
if ((size0 < 0.0f) && (size1 < 0.0f)) {
return 0;
}
if ((size0 * size1) < 0.0f) {
inside = false;
break;
}
i0 = i;
}
}
dgFloat32 convexSphapeUmbra = dgMax (proxy.m_instance0->GetUmbraClipSize(), boxSize.GetScalar());
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping(boxOrigin, convexSphapeUmbra);
m_faceClipSize = proxy.m_instance0->m_childShape->GetBoxMaxRadius();
}
const dgInt32 hullId = proxy.m_instance0->GetUserDataID();
if (inside & !proxy.m_intersectionTestOnly) {
const dgMatrix& matrixInstance0 = proxy.m_instance0->m_globalMatrix;
dgVector normalInHull(matrixInstance0.UnrotateVector(m_normal.Scale4(dgFloat32(-1.0f))));
dgVector pointInHull(proxy.m_instance0->SupportVertex(normalInHull, NULL));
dgVector p0 (matrixInstance0.TransformVector(pointInHull));
dgFloat32 timetoImpact = dgFloat32(0.0f);
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32(0.0f)) {
timetoImpact = penetration / (relativeVelocity % m_normal);
dgAssert(timetoImpact >= dgFloat32(0.0f));
}
if (timetoImpact <= proxy.m_timestep) {
dgVector contactPoints[64];
contactJoint->m_closestDistance = penetration;
proxy.m_timestep = timetoImpact;
proxy.m_normal = m_normal;
proxy.m_closestPointBody0 = p0;
proxy.m_closestPointBody1 = p0 + m_normal.Scale4(penetration);
if (!proxy.m_intersectionTestOnly) {
pointInHull -= normalInHull.Scale4 (DG_ROBUST_PLANE_CLIP);
count = proxy.m_instance0->CalculatePlaneIntersection(normalInHull, pointInHull, contactPoints);
dgVector step(relativeVelocity.Scale4(timetoImpact));
penetration = dgMax(penetration, dgFloat32(0.0f));
dgContactPoint* const contactsOut = proxy.m_contacts;
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_point = matrixInstance0.TransformVector(contactPoints[i]) + step;
contactsOut[i].m_normal = m_normal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
}
}
} else {
m_vertexCount = dgUnsigned16 (m_count);
count = world->CalculateConvexToConvexContacts(proxy);
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
return count;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
dgInt32 count = 0;
dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());
const dgCollisionInstance* const polygonInstance = proxy.m_instance1;
dgAssert(this == polygonInstance->GetChildShape());
dgAssert(m_count);
dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgMatrix& hullMatrix = proxy.m_instance0->m_globalMatrix;
dgContact* const contactJoint = proxy.m_contactJoint;
const dgCollisionInstance* const hull = proxy.m_instance0;
dgVector normalInHull(hullMatrix.UnrotateVector(m_normal));
dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL));
dgVector p0(hullMatrix.TransformVector(pointInHull));
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32(0.0f)) {
return 0;
}
dgVector p1(hullMatrix.TransformVector(hull->SupportVertex(normalInHull, NULL)));
contactJoint->m_closestDistance = dgFloat32(0.0f);
dgFloat32 distance = (m_localPoly[0] - p1) % m_normal;
if (distance >= dgFloat32(0.0f)) {
return 0;
}
dgVector boxSize (hull->GetBoxSize() & dgVector::m_triplexMask);
dgVector boxOrigin ((hull->GetBoxOrigin() & dgVector::m_triplexMask) + dgVector::m_wOne);
bool inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i++) {
dgVector e(m_localPoly[i] - m_localPoly[i0]);
dgVector edgeBoundaryNormal(m_normal * e);
dgPlane plane(edgeBoundaryNormal, - m_localPoly[i0].DotProduct4 (edgeBoundaryNormal).GetScalar());
plane = hullMatrix.TransformPlane(plane);
dgFloat32 supportDist = boxSize.DotProduct4 (plane.Abs()).GetScalar();
dgFloat32 centerDist = plane.DotProduct4 (boxOrigin).GetScalar();
if ((centerDist + supportDist) < dgFloat32(0.0f)) {
return 0;
}
if ((centerDist - supportDist) < dgFloat32(0.0f)) {
inside = false;
break;
}
i0 = i;
}
//inside = false;
dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize();
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping(dgVector(dgFloat32(0.0f)), convexSphapeUmbra);
m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
}
const dgInt32 hullId = hull->GetUserDataID();
if (inside & !proxy.m_intersectionTestOnly) {
dgAssert(penetration >= dgFloat32(0.0f));
dgVector contactPoints[64];
dgAssert(penetration >= 0.0f);
dgVector point(pointInHull + normalInHull.Scale4(penetration + DG_ROBUST_PLANE_CLIP));
count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, contactPoints);
dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f)));
dgContactPoint* const contactsOut = proxy.m_contacts;
dgAssert(contactsOut);
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_point = hullMatrix.TransformVector(contactPoints[i] + step);
contactsOut[i].m_normal = m_normal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
} else {
m_vertexCount = dgUnsigned16 (m_count);
count = world->CalculateConvexToConvexContacts(proxy);
dgAssert(proxy.m_intersectionTestOnly || (count >= 0));
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
if (m_closestFeatureType == 3) {
for (dgInt32 i = 0; i < count; i++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
dgVector normal (contactsOut[0].m_normal);
if (normal.DotProduct4(m_normal).GetScalar() < dgFloat32(0.9995f)) {
dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
dgVector adjacentNormal (CalculateGlobalNormal (parentMesh, dgVector(&m_vertex[index * m_stride])));
if ((m_normal.DotProduct4(adjacentNormal).GetScalar() > dgFloat32(0.9995f))) {
normal = adjacentNormal;
} else {
dgVector dir0(adjacentNormal * m_normal);
dgVector dir1(adjacentNormal * normal);
dgFloat32 projection = dir0.DotProduct4(dir1).GetScalar();
if (projection <= dgFloat32(0.0f)) {
normal = adjacentNormal;
}
}
normal = polygonInstance->m_globalMatrix.RotateVector(normal);
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_normal = normal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
}
return count;
}
void setupPhysics(hkpWorld* physicsWorld)
{
//
// Create the ground box
//
{
hkVector4 groundRadii( 70.0f, 2.0f, 140.0f );
hkpConvexShape* shape = new hkpBoxShape( groundRadii , 0 );
hkpRigidBodyCinfo ci;
ci.m_shape = shape;
ci.m_motionType = hkpMotion::MOTION_FIXED;
ci.m_position = hkVector4( 0.0f, -2.0f, 0.0f );
ci.m_qualityType = HK_COLLIDABLE_QUALITY_FIXED;
physicsWorld->addEntity( new hkpRigidBody( ci ) )->removeReference();
shape->removeReference();
}
hkVector4 groundPos( 0.0f, 0.0f, 0.0f );
hkVector4 posy = groundPos;
//
// Create the walls
//
int wallHeight = 8;
int wallWidth = 8;
int numWalls = 6;
hkVector4 boxSize( 1.0f, 0.5f, 0.5f);
hkpBoxShape* box = new hkpBoxShape( boxSize , 0 );
box->setRadius( 0.0f );
hkReal deltaZ = 25.0f;
posy(2) = -deltaZ * numWalls * 0.5f;
for ( int y = 0; y < numWalls; y ++ ) // first wall
{
createBrickWall( physicsWorld, wallHeight, wallWidth, posy, 0.2f, box, boxSize );
posy(2) += deltaZ;
}
box->removeReference();
//
// Create a ball moving towards the walls
//
const hkReal radius = 1.5f;
const hkReal sphereMass = 150.0f;
hkVector4 relPos( 0.0f,radius + 0.0f, 50.0f );
hkpRigidBodyCinfo info;
hkpMassProperties massProperties;
hkpInertiaTensorComputer::computeSphereVolumeMassProperties(radius, sphereMass, massProperties);
info.m_mass = massProperties.m_mass;
info.m_centerOfMass = massProperties.m_centerOfMass;
info.m_inertiaTensor = massProperties.m_inertiaTensor;
info.m_shape = new hkpSphereShape( radius );
info.m_position.setAdd4(posy, relPos );
info.m_motionType = hkpMotion::MOTION_BOX_INERTIA;
info.m_qualityType = HK_COLLIDABLE_QUALITY_BULLET;
hkpRigidBody* sphereRigidBody = new hkpRigidBody( info );
g_ball = sphereRigidBody;
physicsWorld->addEntity( sphereRigidBody );
sphereRigidBody->removeReference();
info.m_shape->removeReference();
hkVector4 vel( 0.0f,4.9f, -100.0f );
sphereRigidBody->setLinearVelocity( vel );
}
UserCollisionFilterDemo::UserCollisionFilterDemo( hkDemoEnvironment* env)
: hkDefaultPhysicsDemo(env)
{
//
// Setup the camera
//
{
hkVector4 from(10.0f, 10.0f, 15.0f);
hkVector4 to(0.0f, 3.0f, 0.0f);
hkVector4 up(0.0f, 1.0f, 0.0f);
setupDefaultCameras( env, from, to, up );
}
//
// Create the world
//
{
hkpWorldCinfo info;
m_world = new hkpWorld( info );
m_world->lock();
setupGraphics();
}
//
// Register the box-box collision agent
//
{
hkpAgentRegisterUtil::registerAllAgents( m_world->getCollisionDispatcher() );
}
/// To add the filter to the world we simply instantiate a new filter and set it to be active:
//
// Create the my collision filter
//
{
MyCollisionFilter* filter = new MyCollisionFilter();
m_world->setCollisionFilter( filter );
filter->removeReference();
}
// NOTE: You must set the collision filter prior to adding any entities to the world otherwise the pre-filter
// added entities could cause undesirable behaviour before the filter is active.
// All three objects are hkpBoxShape shapes, two of which are fixed and the third dynamic. In order to allow
// our filters work correctly we must assign each of the objects a collision 'group'. Both the lower floor
// and the dynamic box have their collision 'group' set to zero:
//
// Create the floor with collisioninfo = 0
//
{
hkpRigidBodyCinfo info;
hkVector4 fixedBoxSize(5.0f, .5f , 5.0f );
hkpBoxShape* fixedBoxShape = new hkpBoxShape( fixedBoxSize , 0 );
info.m_shape = fixedBoxShape;
info.m_motionType = hkpMotion::MOTION_FIXED;
info.m_position.set(0.0f, -1.0f, 0.0f);
// Create fixed box
hkpRigidBody* floor = new hkpRigidBody(info);
floor->setCollisionFilterInfo(0);
m_world->addEntity(floor);
floor->removeReference();
fixedBoxShape->removeReference();
}
// Whereas the upper floor belongs to the 'one' group:
//
// Create the floor with collisioninfo = 1
//
{
hkpRigidBodyCinfo info;
hkVector4 fixedBoxSize(3.0f, .5f , 3.0f );
hkpBoxShape* fixedBoxShape = new hkpBoxShape( fixedBoxSize , 0 );
info.m_shape = fixedBoxShape;
info.m_motionType = hkpMotion::MOTION_FIXED;
info.m_position.set(0.0f,4.0f,0.0f);
// Create fixed box
hkpRigidBody* floor = new hkpRigidBody(info);
fixedBoxShape->removeReference();
floor->setCollisionFilterInfo(1);
m_world->addEntity(floor);
floor->removeReference();
}
// In this way only the lower floor and the dynamic box will interact based on the rule we defined in
// MyCollisionFilter::isCollisionEnabled(...).
//
// Create a moving box with collisioninfo = 0, so it will collide with only the "bottom" floor
//
{
hkpRigidBodyCinfo boxInfo;
hkVector4 boxSize( .5f, .5f ,.5f );
hkpShape* boxShape = new hkpBoxShape( boxSize , 0 );
boxInfo.m_shape = boxShape;
boxInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;
// Compute the box inertia tensor
hkReal boxMass = 1.0f;
hkpMassProperties massProperties;
hkpInertiaTensorComputer::computeBoxVolumeMassProperties( boxSize, boxMass, massProperties );
boxInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;
boxInfo.m_mass = boxMass;
boxInfo.m_inertiaTensor = massProperties.m_inertiaTensor;
boxInfo.m_position.set(0.0f, 7.0f, 0.0f);
hkpRigidBody* boxRigidBody = new hkpRigidBody( boxInfo );
boxShape->removeReference();
boxRigidBody->setCollisionFilterInfo(0); // HERE WE SET THE COLLISION FILTER INFO
m_world->addEntity( boxRigidBody );
boxRigidBody->removeReference();
}
m_world->unlock();
}
dgInt32 dgConvexHull4d::InitVertexArray(dgHullVector* const points, const dgBigVector* const vertexCloud, dgInt32 count, void* const memoryPool, dgInt32 maxMemSize)
{
for (dgInt32 i = 0; i < count; i ++) {
points[i] = vertexCloud[i];
points[i].m_index = i;
points[i].m_mark = 0;
}
dgSort(points, count, ConvexCompareVertex);
dgInt32 indexCount = 0;
for (int i = 1; i < count; i ++) {
for (; i < count; i ++) {
if (ConvexCompareVertex (&points[indexCount], &points[i], NULL)) {
indexCount ++;
points[indexCount] = points[i];
break;
}
}
}
count = indexCount + 1;
if (count < 4) {
m_count = 0;
return count;
}
dgAABBPointTree4d* tree = BuildTree (NULL, points, count, 0, (dgInt8**) &memoryPool, maxMemSize);
dgBigVector boxSize (tree->m_box[1] - tree->m_box[0]);
boxSize.m_w = dgFloat64 (0.0f);
m_diag = dgFloat32 (sqrt (boxSize.DotProduct4(boxSize).m_x));
m_points[4].m_x = dgFloat64 (0.0f);
dgHullVector* const convexPoints = &m_points[0];
dgStack<dgBigVector> normalArrayPool (256);
dgBigVector* const normalArray = &normalArrayPool[0];
dgInt32 normalCount = BuildNormalList (&normalArray[0]);
dgInt32 index = SupportVertex (&tree, points, normalArray[0]);
convexPoints[0] = points[index];
points[index].m_mark = 1;
bool validTetrahedrum = false;
dgBigVector e1 (dgFloat64 (0.0f), dgFloat64 (0.0f), dgFloat64 (0.0f), dgFloat64 (0.0f)) ;
for (dgInt32 i = 1; i < normalCount; i ++) {
dgInt32 index = SupportVertex (&tree, points, normalArray[i]);
dgAssert (index >= 0);
e1 = points[index] - convexPoints[0];
e1.m_w = dgFloat64 (0.0f);
dgFloat64 error2 = e1.DotProduct4(e1).m_x;
if (error2 > (dgFloat32 (1.0e-4f) * m_diag * m_diag)) {
convexPoints[1] = points[index];
points[index].m_mark = 1;
validTetrahedrum = true;
break;
}
}
if (!validTetrahedrum) {
m_count = 0;
return count;
}
dgInt32 bestIndex = -1;
dgFloat64 bestValue = dgFloat64 (1.0f);
validTetrahedrum = false;
dgFloat64 lenght2 = e1.DotProduct4(e1).m_x;
dgBigVector e2(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));;
for (dgInt32 i = 2; i < normalCount; i ++) {
dgInt32 index = SupportVertex (&tree, points, normalArray[i]);
dgAssert (index >= 0);
dgAssert (index < count);
e2 = points[index] - convexPoints[0];
e2.m_w = dgFloat64 (0.0f);
dgFloat64 den = e2.DotProduct4(e2).m_x;
if (fabs (den) > (dgFloat64 (1.0e-6f) * m_diag)) {
den = sqrt (lenght2 * den);
dgFloat64 num = e2.DotProduct4(e1).m_x;
dgFloat64 cosAngle = fabs (num / den);
if (cosAngle < bestValue) {
bestValue = cosAngle;
bestIndex = index;
}
if (cosAngle < 0.9f) {
break;
}
}
}
if (bestValue < dgFloat64 (0.999f)) {
convexPoints[2] = points[bestIndex];
points[bestIndex].m_mark = 1;
validTetrahedrum = true;
}
if (!validTetrahedrum) {
m_count = 0;
return count;
}
validTetrahedrum = false;
dgBigVector e3(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));;
for (dgInt32 i = 3; i < normalCount; i ++) {
dgInt32 index = SupportVertex (&tree, points, normalArray[i]);
dgAssert (index >= 0);
dgAssert (index < count);
e3 = points[index] - convexPoints[0];
e3.m_w = dgFloat64 (0.0f);
dgFloat64 volume = (e1 * e2) % e3;
if (fabs (volume) > (dgFloat64 (1.0e-4f) * m_diag * m_diag * m_diag)) {
convexPoints[3] = points[index];
points[index].m_mark = 1;
validTetrahedrum = true;
break;
}
}
m_count = 4;
if (!validTetrahedrum) {
m_count = 0;
}
return count;
}
void Restitution (DemoEntityManager* const scene)
{
scene->CreateSkyBox();
// customize the scene after loading
// set a user friction variable in the body for variable friction demos
// later this will be done using LUA script
NewtonWorld* const world = scene->GetNewton();
dMatrix offsetMatrix (dGetIdentityMatrix());
int defaultMaterialID = NewtonMaterialGetDefaultGroupID (world);
NewtonMaterialSetCollisionCallback (world, defaultMaterialID, defaultMaterialID, NULL, UserContactRestitution);
CreateLevelMesh (scene, "flatPlane.ngd", 0);
dVector location (0.0f, 0.0f, 0.0f, 0.0f);
dVector size (0.5f, 0.5f, 1.0f, 0.0f);
// create some spheres
dVector sphSize (1.0f, 1.0f, 1.0f, 0.0f);
NewtonCollision* const sphereCollision = CreateConvexCollision (world, offsetMatrix, sphSize, _SPHERE_PRIMITIVE, 0);
DemoMesh* const sphereMesh = new DemoMesh("sphere", sphereCollision, "smilli.tga", "smilli.tga", "smilli.tga");
// create some boxes too
dVector boxSize (1.0f, 0.5f, 2.0f, 0.0f);
NewtonCollision* const boxCollision = CreateConvexCollision (world, offsetMatrix, boxSize, _BOX_PRIMITIVE, 0);
DemoMesh* const boxMesh = new DemoMesh("box", boxCollision, "smilli.tga", "smilli.tga", "smilli.tga");
int zCount = 10;
dFloat spacing = 4.0f;
dMatrix matrix (dGetIdentityMatrix());
dVector origin (matrix.m_posit);
origin.m_x -= 0.0f;
// create a simple scene
for (int i = 0; i < zCount; i ++) {
dFloat z;
dFloat x;
dFloat mass;
dVector size (1.0f, 0.5f, 2.0f, 0.0f);
x = origin.m_x;
z = origin.m_z + (i - zCount / 2) * spacing;
mass = 1.0f;
matrix.m_posit = FindFloor (world, dVector (x, 100.0f, z), 200.0f);
matrix.m_posit.m_w = 1.0f;
float restitution;
NewtonBody* body;
NewtonCollision* collision;
matrix.m_posit.m_y += 4.0f;
body = CreateSimpleSolid (scene, sphereMesh, mass, matrix, sphereCollision, defaultMaterialID);
NewtonBodySetLinearDamping (body, 0.0f);
collision = NewtonBodyGetCollision(body);
restitution = i * 0.1f + 0.083f;
NewtonCollisionSetUserData (collision, *((void**)&restitution));
matrix.m_posit.m_y += 4.0f;
//body = CreateSimpleSolid (scene, sphereMesh, mass, matrix, sphereCollision, defaultMaterialID, shapeOffsetMatrix);
body = CreateSimpleSolid (scene, boxMesh, mass, matrix, boxCollision, defaultMaterialID);
NewtonBodySetLinearDamping (body, 0.0f);
collision = NewtonBodyGetCollision(body);
restitution = i * 0.1f + 0.083f;
NewtonCollisionSetUserData (collision, *((void**)&restitution));
matrix.m_posit.m_y += 4.0f;
body = CreateSimpleSolid (scene, sphereMesh, mass, matrix, sphereCollision, defaultMaterialID);
NewtonBodySetLinearDamping (body, 0.0f);
collision = NewtonBodyGetCollision(body);
restitution = i * 0.1f + 0.083f;
NewtonCollisionSetUserData (collision, *((void**)&restitution));
matrix.m_posit.m_y += 4.0f;
dVector boxSize (1.0f, 0.5f, 2.0f, 0.0f);
body = CreateSimpleSolid (scene, boxMesh, mass, matrix, boxCollision, defaultMaterialID);
NewtonBodySetLinearDamping (body, 0.0f);
collision = NewtonBodyGetCollision(body);
restitution = i * 0.1f + 0.083f;
NewtonCollisionSetUserData (collision, *((void**)&restitution));
}
boxMesh->Release();
sphereMesh->Release();
NewtonDestroyCollision(boxCollision);
NewtonDestroyCollision(sphereCollision);
dMatrix camMatrix (dGetIdentityMatrix());
dQuaternion rot (camMatrix);
origin = dVector (-25.0f, 5.0f, 0.0f, 0.0f);
scene->SetCameraMatrix(rot, origin);
}
std::unique_ptr<Shape> Shape::createShape(const BasicShape* basicShape, const LayoutSize& logicalBoxSize, WritingMode writingMode, float margin)
{
ASSERT(basicShape);
bool horizontalWritingMode = isHorizontalWritingMode(writingMode);
float boxWidth = horizontalWritingMode ? logicalBoxSize.width() : logicalBoxSize.height();
float boxHeight = horizontalWritingMode ? logicalBoxSize.height() : logicalBoxSize.width();
std::unique_ptr<Shape> shape;
switch (basicShape->type()) {
case BasicShape::BasicShapeCircleType: {
const BasicShapeCircle* circle = static_cast<const BasicShapeCircle*>(basicShape);
float centerX = floatValueForCenterCoordinate(circle->centerX(), boxWidth);
float centerY = floatValueForCenterCoordinate(circle->centerY(), boxHeight);
float radius = circle->floatValueForRadiusInBox(boxWidth, boxHeight);
FloatPoint logicalCenter = physicalPointToLogical(FloatPoint(centerX, centerY), logicalBoxSize.height(), writingMode);
shape = createCircleShape(logicalCenter, radius);
break;
}
case BasicShape::BasicShapeEllipseType: {
const BasicShapeEllipse* ellipse = static_cast<const BasicShapeEllipse*>(basicShape);
float centerX = floatValueForCenterCoordinate(ellipse->centerX(), boxWidth);
float centerY = floatValueForCenterCoordinate(ellipse->centerY(), boxHeight);
float radiusX = ellipse->floatValueForRadiusInBox(ellipse->radiusX(), centerX, boxWidth);
float radiusY = ellipse->floatValueForRadiusInBox(ellipse->radiusY(), centerY, boxHeight);
FloatPoint logicalCenter = physicalPointToLogical(FloatPoint(centerX, centerY), logicalBoxSize.height(), writingMode);
shape = createEllipseShape(logicalCenter, FloatSize(radiusX, radiusY));
break;
}
case BasicShape::BasicShapePolygonType: {
const BasicShapePolygon& polygon = *static_cast<const BasicShapePolygon*>(basicShape);
const Vector<Length>& values = polygon.values();
size_t valuesSize = values.size();
ASSERT(!(valuesSize % 2));
std::unique_ptr<Vector<FloatPoint>> vertices = std::make_unique<Vector<FloatPoint>>(valuesSize / 2);
for (unsigned i = 0; i < valuesSize; i += 2) {
FloatPoint vertex(
floatValueForLength(values.at(i), boxWidth),
floatValueForLength(values.at(i + 1), boxHeight));
(*vertices)[i / 2] = physicalPointToLogical(vertex, logicalBoxSize.height(), writingMode);
}
shape = createPolygonShape(WTF::move(vertices), polygon.windRule());
break;
}
case BasicShape::BasicShapeInsetType: {
const BasicShapeInset& inset = *static_cast<const BasicShapeInset*>(basicShape);
float left = floatValueForLength(inset.left(), boxWidth);
float top = floatValueForLength(inset.top(), boxHeight);
FloatRect rect(left,
top,
std::max<float>(boxWidth - left - floatValueForLength(inset.right(), boxWidth), 0),
std::max<float>(boxHeight - top - floatValueForLength(inset.bottom(), boxHeight), 0));
FloatRect logicalRect = physicalRectToLogical(rect, logicalBoxSize.height(), writingMode);
FloatSize boxSize(boxWidth, boxHeight);
FloatSize topLeftRadius = physicalSizeToLogical(floatSizeForLengthSize(inset.topLeftRadius(), boxSize), writingMode);
FloatSize topRightRadius = physicalSizeToLogical(floatSizeForLengthSize(inset.topRightRadius(), boxSize), writingMode);
FloatSize bottomLeftRadius = physicalSizeToLogical(floatSizeForLengthSize(inset.bottomLeftRadius(), boxSize), writingMode);
FloatSize bottomRightRadius = physicalSizeToLogical(floatSizeForLengthSize(inset.bottomRightRadius(), boxSize), writingMode);
FloatRoundedRect::Radii cornerRadii(topLeftRadius, topRightRadius, bottomLeftRadius, bottomRightRadius);
cornerRadii.scale(calcBorderRadiiConstraintScaleFor(logicalRect, cornerRadii));
shape = createInsetShape(FloatRoundedRect(logicalRect, cornerRadii));
break;
}
default:
ASSERT_NOT_REACHED();
}
shape->m_writingMode = writingMode;
shape->m_margin = margin;
return shape;
}
SlidingWorldDemo::SlidingWorldDemo(hkDemoEnvironment* env)
: hkDefaultPhysicsDemo(env), m_time(0.0f), m_ticks(1), m_currentMode(MANUAL_SHIFT), m_delayBetweenAutomaticShifts(90)
{
// Disable warnings:
hkError::getInstance().setEnabled(0xf0ff00a1, false); //'Attempting to remove an entity twice)'
// Set up the camera.
{
hkVector4 from(0.0f, 75.0f, 30.0f);
hkVector4 to(0.0f, 3.0f, 0.0f);
hkVector4 up(0.0f, 1.0f, 0.0f);
setupDefaultCameras( env, from, to, up );
}
m_centers[0].set(10, 0, 0);
m_centers[1].set(10, 0, 10);
m_centers[2].set(0, 0, 10);
m_centers[3].set(-10, 0, 10);
m_centers[4].set(-10, 0, 0);
m_centers[5].set(-10, 0, -10);
m_centers[6].set(0, 0, -10);
m_centers[7].set(10, 0, -10);
m_currentCenter.setAll(0);
// Create the world.
{
hkpWorldCinfo info;
info.setupSolverInfo(hkpWorldCinfo::SOLVER_TYPE_4ITERS_MEDIUM);
//info.setBroadPhaseWorldSize( 120.0f );
info.setBroadPhaseWorldSize( 30.0f );
m_world = new hkpWorld( info );
m_world->m_wantDeactivation = true;
m_world->lock();
// N.B. We need this border 'for safety': it should play no part in the 'broadphase moving' or the 'coordinate shfting' -
// the border callbacks are *not* fired as a result of calls to hkBroadphase::shiftBroadPhase() or hkBroadphase::shiftAllObjects().
// They will be fired as a result of body addition or world stepping, so this will ensure that bodies added or moved
// (under integration) get correctly removed from simulation so that the only body not fully contained inside the broadphase
// is a single fixed body (assumed to be a large, level-encompassing landscape tile.
SlidingWorldBroadphaseBorder *border = new SlidingWorldBroadphaseBorder( m_world );
m_world->setBroadPhaseBorder(border);
border->removeReference();
// Setup the rest of the default viewers:
setupGraphics();
// Register all collision agents
hkpAgentRegisterUtil::registerAllAgents( m_world->getCollisionDispatcher() );
}
// Create the fixed floor box.
{
hkVector4 fixedBoxSize(30.0f, .5f , 30.0f );
hkpRigidBodyCinfo info;
info.m_shape = new hkpBoxShape( fixedBoxSize , 0 );
info.m_motionType = hkpMotion::MOTION_FIXED;
// Create fixed box.
hkpRigidBody* box = new hkpRigidBody(info);
m_world->addEntity(box)->removeReference();
info.m_shape->removeReference();
}
{
// Create a single shape, and reuse it for all bodies.
hkVector4 boxSize( .75f, .75f ,.75f );
hkpShape* boxShape = new hkpBoxShape( boxSize , 0 );
hkpRigidBodyCinfo boxInfo;
boxInfo.m_shape = boxShape;
boxInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;
// Compute the box inertia tensor.
hkpInertiaTensorComputer::setShapeVolumeMassProperties(boxInfo.m_shape, 5.0f, boxInfo);
boxInfo.m_rigidBodyDeactivatorType = hkpRigidBodyDeactivator::DEACTIVATOR_NEVER;
for (int i = -20; i <= 20; i += 5)
{
for (int j = -20; j <= 20; j += 5)
{
boxInfo.m_position.set( (hkReal)i, 2.0f, (hkReal)j );
hkpRigidBody* boxRigidBody = new hkpRigidBody(boxInfo);
m_world->addEntity( boxRigidBody);
m_boxes.pushBack(boxRigidBody);
}
}
boxShape->removeReference();
}
m_world->unlock();
}
PassOwnPtr<Shape> Shape::createShape(const BasicShape* basicShape, const LayoutSize& logicalBoxSize, WritingMode writingMode, float margin)
{
ASSERT(basicShape);
bool horizontalWritingMode = isHorizontalWritingMode(writingMode);
float boxWidth = horizontalWritingMode ? logicalBoxSize.width().toFloat() : logicalBoxSize.height().toFloat();
float boxHeight = horizontalWritingMode ? logicalBoxSize.height().toFloat() : logicalBoxSize.width().toFloat();
OwnPtr<Shape> shape;
switch (basicShape->type()) {
case BasicShape::BasicShapeCircleType: {
const BasicShapeCircle* circle = toBasicShapeCircle(basicShape);
FloatPoint center = floatPointForCenterCoordinate(circle->centerX(), circle->centerY(), FloatSize(boxWidth, boxHeight));
float radius = circle->floatValueForRadiusInBox(FloatSize(boxWidth, boxHeight));
FloatPoint logicalCenter = physicalPointToLogical(center, logicalBoxSize.height().toFloat(), writingMode);
shape = createCircleShape(logicalCenter, radius);
break;
}
case BasicShape::BasicShapeEllipseType: {
const BasicShapeEllipse* ellipse = toBasicShapeEllipse(basicShape);
FloatPoint center = floatPointForCenterCoordinate(ellipse->centerX(), ellipse->centerY(), FloatSize(boxWidth, boxHeight));
float radiusX = ellipse->floatValueForRadiusInBox(ellipse->radiusX(), center.x(), boxWidth);
float radiusY = ellipse->floatValueForRadiusInBox(ellipse->radiusY(), center.y(), boxHeight);
FloatPoint logicalCenter = physicalPointToLogical(center, logicalBoxSize.height().toFloat(), writingMode);
shape = createEllipseShape(logicalCenter, FloatSize(radiusX, radiusY));
break;
}
case BasicShape::BasicShapePolygonType: {
const BasicShapePolygon* polygon = toBasicShapePolygon(basicShape);
const Vector<Length>& values = polygon->values();
size_t valuesSize = values.size();
ASSERT(!(valuesSize % 2));
OwnPtr<Vector<FloatPoint>> vertices = adoptPtr(new Vector<FloatPoint>(valuesSize / 2));
for (unsigned i = 0; i < valuesSize; i += 2) {
FloatPoint vertex(
floatValueForLength(values.at(i), boxWidth),
floatValueForLength(values.at(i + 1), boxHeight));
(*vertices)[i / 2] = physicalPointToLogical(vertex, logicalBoxSize.height().toFloat(), writingMode);
}
shape = createPolygonShape(vertices.release(), polygon->windRule());
break;
}
case BasicShape::BasicShapeInsetType: {
const BasicShapeInset& inset = *toBasicShapeInset(basicShape);
float left = floatValueForLength(inset.left(), boxWidth);
float top = floatValueForLength(inset.top(), boxHeight);
float right = floatValueForLength(inset.right(), boxWidth);
float bottom = floatValueForLength(inset.bottom(), boxHeight);
FloatRect rect(left, top, std::max<float>(boxWidth - left - right, 0), std::max<float>(boxHeight - top - bottom, 0));
FloatRect logicalRect = physicalRectToLogical(rect, logicalBoxSize.height().toFloat(), writingMode);
FloatSize boxSize(boxWidth, boxHeight);
FloatSize topLeftRadius = physicalSizeToLogical(floatSizeForLengthSize(inset.topLeftRadius(), boxSize), writingMode);
FloatSize topRightRadius = physicalSizeToLogical(floatSizeForLengthSize(inset.topRightRadius(), boxSize), writingMode);
FloatSize bottomLeftRadius = physicalSizeToLogical(floatSizeForLengthSize(inset.bottomLeftRadius(), boxSize), writingMode);
FloatSize bottomRightRadius = physicalSizeToLogical(floatSizeForLengthSize(inset.bottomRightRadius(), boxSize), writingMode);
FloatRoundedRect::Radii cornerRadii(topLeftRadius, topRightRadius, bottomLeftRadius, bottomRightRadius);
FloatRoundedRect finalRect(logicalRect, cornerRadii);
finalRect.constrainRadii();
shape = createInsetShape(finalRect);
break;
}
default:
ASSERT_NOT_REACHED();
}
shape->m_writingMode = writingMode;
shape->m_margin = margin;
return shape.release();
}
ExtendedUserDataDemo::ExtendedUserDataDemo( hkDemoEnvironment* env)
: hkDefaultPhysicsDemo(env)
{
//
// Setup the camera
//
{
hkVector4 from(3.0f, 4.0f, 8.0f);
hkVector4 to(0.0f, 1.0f, 0.0f);
hkVector4 up(0.0f, 1.0f, 0.0f);
setupDefaultCameras( env, from, to, up );
}
//
// Create the world
//
{
hkpWorldCinfo info;
info.setupSolverInfo(hkpWorldCinfo::SOLVER_TYPE_4ITERS_MEDIUM);
info.setBroadPhaseWorldSize( 100.0f );
info.m_simulationType = info.SIMULATION_TYPE_CONTINUOUS;
// Turn off deactivation so we can see continuous contact point processing
info.m_enableDeactivation = false;
m_world = new hkpWorld( info );
m_world->lock();
setupGraphics();
}
//
// Register the box-box collision agent
//
{
hkpAgentRegisterUtil::registerAllAgents(m_world->getCollisionDispatcher());
}
//
// Create the floor
//
{
hkpRigidBodyCinfo info;
hkVector4 fixedBoxSize(5.0f, 0.5f , 5.0f );
hkpBoxShape* fixedBoxShape = new hkpBoxShape( fixedBoxSize , 0 );
info.m_shape = fixedBoxShape;
info.m_motionType = hkpMotion::MOTION_FIXED;
info.m_position.setZero4();
// Add some bounce.
info.m_restitution = 0.8f;
info.m_friction = 1.0f;
// Force this to collide on PPU and raise all callbacks
info.m_forceCollideOntoPpu = true;
// Force this to collide on PPU and raise all callbacks
info.m_forceCollideOntoPpu = true;
// Create fixed box
hkpRigidBody* floor = new hkpRigidBody(info);
m_world->addEntity(floor);
floor->removeReference();
fixedBoxShape->removeReference();
}
// For this demo we simply have two box shapes which are constructed in the usual manner using a hkpRigidBodyCinfo 'blueprint'.
// The dynamic box creation code in presented below. There are two key things to note in this example;
// the 'm_restitution' member variable, which we have explicitly set to value of 0.9.
// The restitution is over twice the default value of
// 0.4 and is set to give the box (the floor has been set likewise) a more 'bouncy' nature.
//
// Create a moving object
//
{
hkpRigidBodyCinfo multiSpheresInfo;
hkVector4 boxSize( .5f, .5f ,.5f );
// Build listShape of spheres
//
{
hkpSphereShape* sphere = new hkpSphereShape(0.2f);
hkReal size = 0.5f;
hkpShape* shapes[8];
for (int i = 0; i < 8; i++)
{
hkReal px = i&0x1 ? -size : size;
hkReal py = i&0x2 ? -size : size;
hkReal pz = i&0x4 ? -size : size;
hkVector4 translation(px, py, pz);
shapes[i] = new hkpConvexTranslateShape(sphere, translation);
}
sphere->removeReference();
hkpListShape* list = new hkpListShape(shapes, 8);
for(int i = 0; i < 8; i++)
{
shapes[i]->removeReference();
}
multiSpheresInfo.m_shape = list;
}
// Compute the box inertia tensor
hkpInertiaTensorComputer::setShapeVolumeMassProperties( multiSpheresInfo.m_shape, 1.0f, multiSpheresInfo );
multiSpheresInfo.m_qualityType = HK_COLLIDABLE_QUALITY_CRITICAL;
multiSpheresInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;
// Place the box so it bounces interestingly
multiSpheresInfo.m_position.set(0.0f, 5.0f, 0.0f);
hkVector4 axis(1.0f, 2.0f, 3.0f);
axis.normalize3();
multiSpheresInfo.m_rotation.setAxisAngle(axis, -0.7f);
// Add some bounce.
multiSpheresInfo.m_restitution = 0.5f;
multiSpheresInfo.m_friction = 0.1f;
multiSpheresInfo.m_numUserDatasInContactPointProperties = 3;
hkpRigidBody* multiSphereRigidBody = new hkpRigidBody( multiSpheresInfo );
// remove reference from boxShape since rigid body "owns" it
multiSpheresInfo.m_shape->removeReference();
m_world->addEntity( multiSphereRigidBody );
multiSphereRigidBody->removeReference();
// Add the collision event listener to the rigid body
m_listener = new MyExtendedUserDataListener( multiSphereRigidBody );
}
m_world->unlock();
}