void PhysicsSystem::Update(float _dt) { for (auto it = m_entityMap.begin(); it != m_entityMap.end(); ++it) { Entity* e = it->second; if (e->GetState() != Entity::ALIVE) continue; auto collision = e->GetComponent<CollisionComponent>(); auto stats = e->GetComponent<CollisionStatsComponent>(); auto position = e->GetComponent<PositionComponent>(); auto velocity = e->GetComponent<VelocityComponent>(); auto rotation = e->GetComponent<RotationComponent>(); b2Body* b2Body = collision->GetBody(); // Update position, velocity and rotation after the components if (position && rotation) { const b2Vec2 b2Pos = b2Vec2(position->GetPosition().x, position->GetPosition().y); if (collision->GetBody()->GetPosition().x != b2Pos.x || collision->GetBody()->GetPosition().y != b2Pos.y || collision->GetBody()->GetAngle() != rotation->GetRotation().z) collision->GetBody()->SetTransform(b2Pos, rotation->GetRotation().z); } else if (position) { const b2Vec2 b2Pos = b2Vec2(position->GetPosition().x, position->GetPosition().y); if (collision->GetBody()->GetPosition().x != b2Pos.x || collision->GetBody()->GetPosition().y != b2Pos.y) collision->GetBody()->SetTransform(b2Pos, collision->GetBody()->GetAngle()); } else if (rotation) { if (collision->GetBody()->GetAngle() != rotation->GetRotation().z) collision->GetBody()->SetTransform(collision->GetBody()->GetPosition(), rotation->GetRotation().z); } if (velocity) { const b2Vec2 b2Velocity = b2Vec2(velocity->m_velocity.x, velocity->m_velocity.y); if (collision->GetBody()->GetLinearVelocity().x != b2Velocity.x || collision->GetBody()->GetLinearVelocity().y != b2Velocity.y) collision->GetBody()->SetLinearVelocity(b2Velocity); } // Update velocity min/max and deacceleration if (velocity && stats) { if (b2Body->GetLinearVelocity().y <= 0.5f && b2Body->GetLinearVelocity().y >= -0.5f) b2Body->SetLinearVelocity(b2Vec2(b2Body->GetLinearVelocity().x, -3.0f)); float speed = b2Body->GetLinearVelocity().Length(); // Speed cant be 0 if (speed == 0) continue; // Set speed between min/max if (speed < stats->GetMinSpeed()) { b2Body->SetLinearVelocity(b2Vec2((b2Body->GetLinearVelocity().x / speed) * stats->GetMinSpeed(), (b2Body->GetLinearVelocity().y / speed) * stats->GetMinSpeed())); } if (speed > stats->GetMaxSpeed()) { b2Body->SetLinearVelocity(b2Vec2((b2Body->GetLinearVelocity().x / speed) * stats->GetMaxSpeed(), (b2Body->GetLinearVelocity().y / speed) * stats->GetMaxSpeed())); } // Deaccelerate if (speed > stats->GetMaxDampingSpeed()) { float newSpeed = speed - (stats->GetDampingAcceleration() * _dt); if (speed < stats->GetMaxDampingSpeed()) newSpeed = stats->GetMaxDampingSpeed(); b2Body->SetLinearVelocity(b2Vec2((b2Body->GetLinearVelocity().x / speed) * newSpeed, (b2Body->GetLinearVelocity().y / speed) * newSpeed)); } } } // Simulate worlds physics m_b2World->Step(_dt, VELOCITYITERATIONS, POSITIONITERATIONS); // Update position, velocity and rotation components for (auto it = m_entityMap.begin(); it != m_entityMap.end(); ++it) { Entity* e = it->second; if (e->GetState() != Entity::ALIVE) continue; auto collision = e->GetComponent<CollisionComponent>(); auto position = e->GetComponent<PositionComponent>(); auto velocity = e->GetComponent<VelocityComponent>(); auto rotation = e->GetComponent<RotationComponent>(); b2Body* b2Body = collision->GetBody(); collision->ResetCollisions(); if (position) { b2Vec2 b2Pos = b2Body->GetPosition(); position->SetPosition(VECTOR3(b2Pos.x, b2Pos.y, position->GetPosition().z)); } if (velocity) { b2Vec2 b2Velocity = b2Body->GetLinearVelocity(); velocity->m_velocity = VECTOR3(b2Velocity.x, b2Velocity.y, velocity->m_velocity.z); } if (rotation) { QUAT rot = rotation->GetRotation(); //rotation->SetRotation(QUAT(rot.x, rot.y, b2Body->GetAngle(), rot.w)); } } // Do collisions checks for (b2Contact* contact = m_b2World->GetContactList(); contact; contact = contact->GetNext()) { if (!contact->IsTouching()) continue; b2Fixture* fixtureA = contact->GetFixtureA(); b2Fixture* fixtureB = contact->GetFixtureB(); Entity* entityA = 0; Entity* entityB = 0; for (auto it = m_entityMap.begin(); it != m_entityMap.end(); ++it) { Entity* e = it->second; if (e->GetState() != Entity::ALIVE) continue; if (!entityA && e->GetComponent<CollisionComponent>()->HasBody(fixtureA->GetBody())) entityA = e; else if (!entityB && e->GetComponent<CollisionComponent>()->HasBody(fixtureB->GetBody())) entityB = e; if (entityA && entityB) break; } if (entityA && entityB) { CollisionContact collisionContact = CollisionContact(contact, fixtureA, fixtureB, entityB->GetId()); entityA->GetComponent<CollisionComponent>()->CollidingWith(collisionContact); collisionContact = CollisionContact(contact, fixtureB, fixtureA, entityA->GetId()); entityB->GetComponent<CollisionComponent>()->CollidingWith(collisionContact); } } }
bool DynamicsWorld::castRay( const btVector3 & origin, const btVector3 & direction, const btScalar length, const btCollisionObject * caster, CollisionContact & contact) const { btVector3 p = origin + direction * length; btVector3 n = -direction; btScalar d = length; int patch_id = -1; const Bezier * b = 0; const TrackSurface * s = TrackSurface::None(); const btCollisionObject * c = 0; MyRayResultCallback ray(origin, p, caster); rayTest(origin, p, ray); // track geometry collision if (ray.hasHit()) { p = ray.m_hitPointWorld; n = ray.m_hitNormalWorld; d = ray.m_closestHitFraction * length; c = ray.m_collisionObject; if (c->isStaticObject()) { TrackSurface * ts = static_cast<TrackSurface*>(c->getUserPointer()); if (c->getCollisionShape()->isCompound()) ts = static_cast<TrackSurface*>(ray.m_shape->getUserPointer()); // verify surface pointer if (track) { const std::vector<TrackSurface> & surfaces = track->GetSurfaces(); if (ts < &surfaces[0] || ts > &surfaces[surfaces.size() - 1]) ts = NULL; assert(ts); } if (ts) s = ts; } // track bezierpatch collision if (track) { Vec3 org = ToMathVector<float>(origin); Vec3 dir = ToMathVector<float>(direction); Vec3 colpoint; Vec3 colnormal; patch_id = contact.GetPatchId(); if (track->CastRay(org, dir, length, patch_id, colpoint, b, colnormal)) { p = ToBulletVector(colpoint); n = ToBulletVector(colnormal); d = (colpoint - org).Magnitude(); } } contact = CollisionContact(p, n, d, patch_id, b, s, c); return true; } // should only happen on vehicle rollover contact = CollisionContact(p, n, d, patch_id, b, s, c); return false; }