bool IntersectAABB(AABB *a, AABB *b)
{
if(a->top_left.x > b->top_left.x)
return IntersectAABB(b, a);
return a->bottom_right.x > b->top_left.x && !(a->bottom_right.y > b->top_left.y || b->bottom_right.y > a->top_left.y);
}
u8 Scene::Cast(vec3 o, const vec3& d){
vec3 inv; // TODO: Cache
inv.x = 1.0f / d.x;
inv.y = 1.0f / d.y;
inv.z = 1.0f / d.z;
auto inx = IntersectAABB(o, inv, vec3{0,0,0}, vec3{width, height, depth});
if(!inx.intersect) return 0;
o += d * inx.t;
for(u32 i = 0; i < 500; i++){
auto b = Test(o);
if(b > 0) return b;
o += d*0.1f;
}
return 0;
}
void Physics::Update(double delta_time)
{
last_physics_update += delta_time;
for(std::list<RigidBody*>::iterator it = register_queue.begin(); it != register_queue.end();)
physics_objects.push_back(*(it++));
register_queue.clear();
for(std::list<RigidBody*>::iterator it = unregister_queue.begin(); it != unregister_queue.end();)
physics_objects.erase(std::remove(physics_objects.begin(), physics_objects.end(), *(it++)), physics_objects.end());
unregister_queue.clear();
if(last_physics_update < update_time)
return;
for(unsigned int i = 0; i < physics_objects.size(); i++ )
for(unsigned int j = i+1; j < physics_objects.size(); j++ )
{
if(IntersectAABB(&physics_objects[i]->aabb, &physics_objects[j]->aabb))
{
vec3 contact_normal;
vec3 contact_point;
if(IntersectConvexShape(physics_objects[i]->shape, physics_objects[i]->model, physics_objects[j]->shape, physics_objects[j]->model, contact_point, contact_normal))
{
if(physics_objects[i]->state == PO_STATIC && physics_objects[j]->state != PO_STATIC)
physics_objects[j]->model.position -= contact_normal;
else if(physics_objects[j]->state == PO_STATIC && physics_objects[i]->state != PO_STATIC)
physics_objects[i]->model.position += contact_normal;
else if(physics_objects[j]->state != PO_STATIC && physics_objects[i]->state != PO_STATIC)
{
if(abs(physics_objects[i]->anti_mass + physics_objects[j]->anti_mass) < math_epsilon)
{
physics_objects[j]->model.position -= contact_normal/2.0f;
physics_objects[i]->model.position += contact_normal/2.0f;
}
else
{
float summ_antimass = (float)(physics_objects[j]->anti_mass + physics_objects[i]->anti_mass);
physics_objects[j]->model.position -= contact_normal * (float)(physics_objects[j]->anti_mass / summ_antimass);
physics_objects[i]->model.position += contact_normal * (float)(physics_objects[i]->anti_mass / summ_antimass);
}
}
double impulse = CalculateImpulse(contact_normal, physics_objects[j], physics_objects[i], contact_point);
if(physics_objects[i]->state != PO_STATIC)
physics_objects[i]->ApplyImpulse(contact_point, contact_normal, -impulse);
if(physics_objects[j]->state != PO_STATIC)
physics_objects[j]->ApplyImpulse(contact_point, contact_normal, impulse);
physics_objects[i]->OnCollide(physics_objects[j]);
physics_objects[j]->OnCollide(physics_objects[i]);
}
}
}
for(unsigned int i = 0; i < physics_objects.size(); i++)
physics_objects[i]->Tick(last_physics_update);
last_physics_update = 0;
}
int Engine::DepthRaytrace( Ray& a_Ray, iAVec3f & a_Acc, int a_Depth, float a_RIndex, float& a_Dist, RayPenetration * ray_p, std::vector<Intersection*> &vecIntersections, traverse_stack * stack, bool dipAsColor )
{
if (a_Depth > s->TRACEDEPTH) return 0;
// trace primary ray
a_Dist = 1000000.0f;
iAVec3f pi;
std::vector<intersection*> intersections;
// find intersections
unsigned int cutAABBListSize;
if(m_cutAABBList)
cutAABBListSize = m_cutAABBListSize;
else
cutAABBListSize = 0;
if(cutAABBListSize)
{
bool intersects = false;
for (unsigned int i=0; i<cutAABBListSize; i++)
{
float a,b;
//if(IntersectCyl(a_Ray, *((*m_cutAABBList)[i]), a, b, 1))
if(IntersectAABB(a_Ray, *((*m_cutAABBList)[i]), a, b))
{
intersects = true;
break;
}
}
if(!intersects)
return 0;
}
m_Scene->getBSPTree()->GetIntersectionsNR(a_Ray, intersections,stack);
if(intersections.size()==0)
return 0;
std::sort(intersections.begin(), intersections.end(), intersectionCompare);
//delete coincident intersections
//it happens when ray hits common edge of 2 neighboring triangles
ray_p->penetrationsSize=0;
ray_p->avDipAng=0;
for (int i=0; i<(int)intersections.size()-1; )
{
/*if( intersections[i+1]->dist == intersections[i]->dist )
{
delete intersections[i]; // Remove the object
intersections[i]=0;
intersections.erase(intersections.begin() + i);
}
else */
//TODO: no triangles repeated?
if( intersections[i+1]->tri->GetIndex() == intersections[i]->tri->GetIndex() )
{
delete intersections[i]; // Remove the object
intersections[i]=0;
intersections.erase(intersections.begin() + i);
}
else
++i;
}
//
unsigned int intetsectSize = (unsigned int) intersections.size();
float penetrationDepth = 0;
//Sometimes it happens, yet lets have this workaround
if(intetsectSize%2 == 0)//TODO: temporary workaround
for (unsigned int i=0; i<intetsectSize; i++)
{
if(i%2==1)
{
float dist = intersections[i]->dist - intersections[i-1]->dist;
penetrationDepth += dist;
//ray_p->penetrations[ray_p->penetrationsSize] = dist;
ray_p->penetrationsSize++;
ray_p->totalPenetrLen += dist;
}
//add intersection in intersections vector
TriPrim* tri = intersections[i]->tri;
Intersection *isec = new Intersection(tri->GetIndex(), tri->GetAngleCos(a_Ray));
vecIntersections.push_back(isec);
ray_p->avDipAng+=fabs(isec->dip_angle);
}
ray_p->avDipAng/=intersections.size();
//rayPenetr->totalPenetrLen/=(rayPenetr->penetrations.size());
for (unsigned int i=0; i<intersections.size(); i++)
delete intersections[i];
float coef = penetrationDepth*s->COLORING_COEF;
if(dipAsColor)
{
a_Acc = iAVec3f((s->COL_RANGE_MIN_R+s->COL_RANGE_DR*(1-ray_p->avDipAng))/255.0,
(s->COL_RANGE_MIN_G+s->COL_RANGE_DG*(1-ray_p->avDipAng))/255.0,
(s->COL_RANGE_MIN_B+s->COL_RANGE_DB*(1-ray_p->avDipAng))/255.0);
}
else
{
a_Acc = iAVec3f(coef, coef, coef);
}
// return pointer to primitive hit by primary ray
return 1;
}
//ブロードフェーズ
void Physics::BroadPhase(RigidbodyState* states, Collider* colliders, unsigned int numRigidbodies,
const Pair *oldPairs, const unsigned int numOldPairs, Pair *newPairs, unsigned int &numNewPairs, const unsigned int maxPairs,
DefaultAllocator* allocator, void *userData, BroadPhaseCallback callback = NULL
){
assert(states);
assert(colliders);
assert(oldPairs);
assert(newPairs);
assert(allocator);
numNewPairs = 0;
// AABB交差ペアを見つける（総当たり）
// TODO:高速化
for (unsigned int i = 0; i<numRigidbodies; i++) {
for (unsigned int j = i + 1; j<numRigidbodies; j++) {
const RigidbodyState &stateA = states[i];
const Collider &collidableA = colliders[i];
const RigidbodyState &stateB = states[j];
const Collider &collidableB = colliders[j];
if (callback && !callback(i, j, userData)) {
continue;
}
Matrix3 orientationA(stateA.m_orientation);
Vector3 centerA = stateA.m_position + orientationA * collidableA.m_center;
Vector3 halfA = absPerElem(orientationA) * (collidableA.m_half + Vector3(AABB_EXPAND));// AABBサイズを若干拡張
Matrix3 orientationB(stateB.m_orientation);
Vector3 centerB = stateB.m_position + orientationB * collidableB.m_center;
Vector3 halfB = absPerElem(orientationB) * (collidableB.m_half + Vector3(AABB_EXPAND));// AABBサイズを若干拡張
if (IntersectAABB(centerA, halfA, centerB, halfB) && numNewPairs < maxPairs) {
//衝突情報を管理するクラスを作成
Pair &newPair = newPairs[numNewPairs++];
newPair.rigidBodyA = i<j ? i : j;
newPair.rigidBodyB = i<j ? j : i;
newPair.contact = NULL;
}
}
}
// ソート
{
Pair *sortBuff = (Pair*)allocator->allocate(sizeof(Pair)*numNewPairs);
Sort<Pair>(newPairs, sortBuff, numNewPairs);
allocator->deallocate(sortBuff);
}
// 新しく検出したペアと過去のペアを比較
Pair *outNewPairs = (Pair*)allocator->allocate(sizeof(Pair)*numNewPairs);
Pair *outKeepPairs = (Pair*)allocator->allocate(sizeof(Pair)*numOldPairs);
assert(outNewPairs);
assert(outKeepPairs);
unsigned int nNew = 0;
unsigned int nKeep = 0;
unsigned int oldId = 0, newId = 0;
while (oldId<numOldPairs&&newId<numNewPairs) {
if (newPairs[newId].key > oldPairs[oldId].key) {
// remove
allocator->deallocate(oldPairs[oldId].contact);
oldId++;
}
else if (newPairs[newId].key == oldPairs[oldId].key) {
// keep
assert(nKeep <= numOldPairs);
outKeepPairs[nKeep] = oldPairs[oldId];
nKeep++;
oldId++;
newId++;
}
else {
// new
assert(nNew <= numNewPairs);
outNewPairs[nNew] = newPairs[newId];
nNew++;
newId++;
}
};
if (newId<numNewPairs) {
// all new
for (; newId<numNewPairs; newId++, nNew++) {
assert(nNew <= numNewPairs);
outNewPairs[nNew] = newPairs[newId];
}
}
else if (oldId<numOldPairs) {
// all remove
for (; oldId<numOldPairs; oldId++) {
allocator->deallocate(oldPairs[oldId].contact);
}
}
for (unsigned int i = 0; i<nNew; i++) {
outNewPairs[i].contact = (Contact*)allocator->allocate(sizeof(Contact));
outNewPairs[i].contact->Reset();
}
for (unsigned int i = 0; i<nKeep; i++) {
outKeepPairs[i].contact->Refresh(
states[outKeepPairs[i].rigidBodyA].m_position,
states[outKeepPairs[i].rigidBodyA].m_orientation,
states[outKeepPairs[i].rigidBodyB].m_position,
states[outKeepPairs[i].rigidBodyB].m_orientation);
}
numNewPairs = 0;
for (unsigned int i = 0; i<nKeep; i++) {
outKeepPairs[i].type = PairTypeKeep;
newPairs[numNewPairs++] = outKeepPairs[i];
}
for (unsigned int i = 0; i<nNew; i++) {
outNewPairs[i].type = PairTypeNew;
newPairs[numNewPairs++] = outNewPairs[i];
}
allocator->deallocate(outKeepPairs);
allocator->deallocate(outNewPairs);
// ソート
{
Pair *sortBuff = (Pair*)allocator->allocate(sizeof(Pair)*numNewPairs);
Sort<Pair>(newPairs, sortBuff, numNewPairs);
allocator->deallocate(sortBuff);
}
}
