void GeomTree::TraceCoherentRays(const BVHNode *currnode, int numRays, const vector3f &a_origin, const vector3f *a_dirs, isect_t *isects) const
{
bvhstack stack[32];
int stackpos = -1;
vector3f *invDirs = static_cast<vector3f*>(alloca(sizeof(vector3f)*numRays));
for (int i=0; i<numRays; i++) {
invDirs[i] = vector3f(1.0f/a_dirs[i].x, 1.0f/a_dirs[i].y, 1.0f/a_dirs[i].z);
}
int activeRay = numRays - 1;
for (;;) {
while (!currnode->IsLeaf()) {
do {
if (SlabsRayAabbTest(currnode, a_origin, invDirs[activeRay], &isects[activeRay])) break;
} while (activeRay-- > 0);
if (activeRay < 0) goto pop_bstack;
stackpos++;
stack[stackpos].node = currnode->kids[1];
stack[stackpos].activeRay = activeRay;
currnode = currnode->kids[0];
}
// triangle intersection j**z
for (int i=0; i<currnode->numTris; i++) {
RayTriIntersect(activeRay+1, a_origin, a_dirs, currnode->triIndicesStart[i], isects);
}
pop_bstack:
if (stackpos < 0) break;
currnode = stack[stackpos].node;
activeRay = stack[stackpos].activeRay;
stackpos--;
}
}
void GeomTree::TraceRay(const BVHNode *currnode, const vector3f &a_origin, const vector3f &a_dir, isect_t *isect) const
{
BVHNode *stack[32];
int stackpos = -1;
vector3f invDir(1.0f/a_dir.x, 1.0f/a_dir.y, 1.0f/a_dir.z);
for (;;) {
while (!currnode->IsLeaf()) {
if (!SlabsRayAabbTest(currnode, a_origin, invDir, isect)) goto pop_bstack;
stackpos++;
stack[stackpos] = currnode->kids[1];
currnode = currnode->kids[0];
}
// triangle intersection j**z
for (int i=0; i<currnode->numTris; i++) {
RayTriIntersect(1, a_origin, &a_dir, currnode->triIndicesStart[i], isect);
}
pop_bstack:
if (stackpos < 0) break;
currnode = stack[stackpos];
stackpos--;
}
}
bool PhysicsEngine::CollisionWithMapSimple(PowerBall* b, Map* map, Vector3 &normalPlane)
{
MaloW::Array<MeshStrip*>* temp = map->GetMesh()->GetStrips();
//int sizeMstrip = temp->size();
int sizeVertexS0 = temp->get(0)->getNrOfVerts();
Vertex* verts;
//Vector3 origin = this->GetPositionVector3();
Vector3 origin = b->GetTempPosition();
Vector3 dir = b->GetVelocity();
Vector3 dirN = dir/dir.GetLength();
verts = temp->get(0)->getVerts();
/*
for(int i = 0;i<sizeMstrip;i++)
{
}
*/
Vector3 p0,p1,p2, normal, v1,v2;
float smalestTime = -1;
bool firstHit = false;
float u, v,t;
float lengthProjN = 0;
Vector3 p0Store, p1Store,p2Store, normalStore;
Vector3 pos = Vector3(map->GetMesh()->GetPosition());
Vector3 posS = b->GetTempPosition();//this->GetPositionVector3();
Vector3 rayDirection;
Vector3 scalingMesh = map->GetMesh()->GetScaling();
D3DXMATRIX quat;
D3DXMatrixRotationQuaternion(&quat, &map->GetMesh()->GetRotation());
Matrix4 rotate(quat);
rotate.TransposeThis();
Matrix4 scaling;
scaling.SetScale(scalingMesh);
Matrix4 translate;
translate.SetTranslate(pos);
Matrix4 world = translate*rotate*scaling;
for(int i =0; i< sizeVertexS0; i+=3)
{
/*
p0 = Vector3(verts[i].pos).GetComponentMultiplication(scalingMesh) + pos;
p1 = Vector3(verts[i+1].pos).GetComponentMultiplication(scalingMesh) +pos;
p2 = Vector3(verts[i+2].pos).GetComponentMultiplication(scalingMesh) + pos;
*/
p0 = world*Vector3(verts[i].pos);
p1 = world*Vector3(verts[i+1].pos);
p2 = world*Vector3(verts[i+2].pos);
v1 = p1-p0;
v2 = p2-p0;
rayDirection = v1.GetCrossProduct(v2);
rayDirection.normalize();
float tempLength;
Vector3 ny;
Vector3 projN;
if(RayTriIntersect(origin , rayDirection, p0, p1, p2, u, v, t) )
{
normal = rayDirection;
ny = origin - p0;
projN = normal*ny.GetDotProduct(normal);
tempLength = projN.GetLength();
if(!firstHit)
{
firstHit = true;
smalestTime = t;
lengthProjN = tempLength;
p0Store = p0;
p1Store = p1;
p2Store = p2;
normalStore = normal;
}
else
{
if( tempLength < lengthProjN )
{
smalestTime = t;
lengthProjN = tempLength;
p0Store = p0;
p1Store = p1;
p2Store = p2;
normalStore = normal;
}
}
}
// check agains all edges
Vector3 lineDirection;
float scalarProj;
Vector3 projOnLine;
Vector3 normalToLine;
// edge 1:
ny = origin - p0;
lineDirection = p1 - p0;
scalarProj = (ny.GetDotProduct(lineDirection)/lineDirection.GetLengthSquared());
projOnLine = lineDirection * scalarProj;
if( (scalarProj >= 0.0f) && (scalarProj <= 1) )
{
normalToLine = ny - projOnLine;
tempLength = normalToLine.GetLength();
if(!firstHit)
{
firstHit = true;
lengthProjN = tempLength;
p0Store = p0;
p1Store = p1;
p2Store = p2;
normalStore = normalToLine;
normalStore.normalize();
}
else
{
if( tempLength < lengthProjN )
{
lengthProjN = tempLength;
p0Store = p0;
p1Store = p1;
p2Store = p2;
normalStore = normalToLine;
normalStore.normalize();
}
}
}
// edge 2:
ny = origin - p1;
lineDirection = p2 - p1;
scalarProj = (ny.GetDotProduct(lineDirection)/lineDirection.GetLengthSquared());
projOnLine = lineDirection * scalarProj;
if( (scalarProj >= 0.0f) && (scalarProj <= 1) )
{
normalToLine = ny - projOnLine;
tempLength = normalToLine.GetLength();
if(!firstHit)
{
firstHit = true;
lengthProjN = tempLength;
p0Store = p0;
p1Store = p1;
p2Store = p2;
normalStore = normalToLine;
normalStore.normalize();
}
else
{
if( tempLength < lengthProjN )
{
lengthProjN = tempLength;
p0Store = p0;
p1Store = p1;
p2Store = p2;
normalStore = normalToLine;
normalStore.normalize();
}
}
}
// edge 3:
ny = origin - p2;
lineDirection = p0 - p2;
scalarProj = (ny.GetDotProduct(lineDirection)/lineDirection.GetLengthSquared());
projOnLine = lineDirection * scalarProj;
if( (scalarProj >= 0.0f) && (scalarProj <= 1) )
{
normalToLine = ny - projOnLine;
tempLength = normalToLine.GetLength();
if(!firstHit)
{
firstHit = true;
lengthProjN = tempLength;
p0Store = p0;
p1Store = p1;
p2Store = p2;
normalStore = normalToLine;
normalStore.normalize();
}
else
{
if( tempLength < lengthProjN )
{
lengthProjN = tempLength;
p0Store = p0;
p1Store = p1;
p2Store = p2;
normalStore = normal;
}
}
}
}
if(firstHit)
{
// for checking if the ball are in the air not turned on at the moment,
float eps = 0.5f; //0.001
if( (lengthProjN < (b->GetRadius() + eps)) && (lengthProjN > (b->GetRadius() - eps)) )
{
b->SetNormalContact(normalStore);
b->SetHasContact(true);
}
else
{
b->SetNormalContact(normalStore);
b->SetHasContact(false);
}
if( lengthProjN <= b->GetRadius())
{
Vector3 velNorm = b->GetVelocity();
velNorm.normalize();
if(normalStore.GetDotProduct(velNorm) >=0)
return false;
float diff = abs(lengthProjN- b->GetRadius());
//Vector3 newPo = origin -dirN*diff;
//Vector3 projVel = normalStore * this->mVelocity.GetDotProduct(normalStore);
Vector3 newPo = origin + normalStore*diff;
/*
if( projVel.GetDotProduct(normalStore) < 0.0f)
{
newPo = origin - normalStore*diff;
return false;
}
else
newPo = origin + normalStore*diff;
*/
//this->SetPosition(newPo);
b->SetTempPosition(newPo);
normalPlane = normalStore;
//this->mNormalContact = normalPlane;
//this->mHasContact = true;
return true;
}
else
{
normalPlane = Vector3(0,0,0);
//this->mNormalContact = normalPlane;
//this->mHasContact = false;
return false;
}
}
normalPlane = Vector3(0,0,0);
b->SetNormalContact(normalPlane);
//this->mHasContact = false;
return false;
}