void PhysWorld3D::ForEachBodyInAABB(const Boxf& box, const BodyIterator& iterator)
{
auto NewtonCallback = [](const NewtonBody* const body, void* const userdata) -> int
{
const BodyIterator& bodyIterator = *static_cast<BodyIterator*>(userdata);
return bodyIterator(*static_cast<RigidBody3D*>(NewtonBodyGetUserData(body)));
};
NewtonWorldForEachBodyInAABBDo(m_world, box.GetMinimum(), box.GetMaximum(), NewtonCallback, const_cast<void*>(static_cast<const void*>(&iterator)));
}
virtual void OnRender (dFloat timestep) const
{
dVector p0(0.0f);
dVector p1(0.0f);
DemoEntity* const entity = (DemoEntity*)NewtonBodyGetUserData(m_body);
const dMatrix& matrix = entity->GetRenderMatrix();
NewtonCollision* const collision = NewtonBodyGetCollision(m_body);
CalculateAABB (collision, matrix, p0, p1);
NewtonWorld* const world = NewtonBodyGetWorld(m_body);
NewtonWorldForEachBodyInAABBDo(world, &p0[0], &p1[0], CalculateContacts, (void*)this);
}
void dVehicleChassis::CalculateTireContacts(dFloat timestep)
{
dComplementaritySolver::dBodyState* const chassisBody = m_vehicle->GetBody();
const dMatrix& matrix = chassisBody->GetMatrix();
dVector origin(matrix.TransformVector(m_obbOrigin));
dVector size(matrix.m_front.Abs().Scale(m_obbSize.m_x) + matrix.m_up.Abs().Scale(m_obbSize.m_y) + matrix.m_right.Abs().Scale(m_obbSize.m_z));
dVector p0 (origin - size);
dVector p1 (origin + size);
dCollectCollidingBodies bodyList(GetBody());
NewtonWorld* const world = NewtonBodyGetWorld(GetBody());
NewtonWorldForEachBodyInAABBDo(world, &p0.m_x, &p1.m_x, OnAABBOverlap, &bodyList);
const dList<dVehicleNode*>& children = m_vehicle->GetChildren();
for (dList<dVehicleNode*>::dListNode* tireNode = children.GetFirst(); tireNode; tireNode = tireNode->GetNext()) {
dVehicleVirtualTire* const tire = (dVehicleVirtualTire*)tireNode->GetInfo()->GetAsTire();
if (tire) {
tire->CalculateContacts(bodyList, timestep);
}
}
}
void PlayerEntity::UpdatePhysics(NewtonWorld* world, float dt)
{
Camera* cam=PlayerEntity::camera;
const float moveSpeed = 100.0*dt;
const float maxAcceleration = 10.0*dt;
const float fallAcceleration = 4.0*dt;
const float jumpStrength = 100.0*dt;
static bool jumpOk;
float move = (float)( glfwGetKey('W') - glfwGetKey('S') );
float strafe = (float)( glfwGetKey('D') - glfwGetKey('A') );
//Desired velocity representerar den önskvärda hastigheten från input
glm::vec3 desiredVelocity(strafe,move,0.0f);
//Normalisera desired om man önskar att gå snett
if(glm::dot(desiredVelocity,desiredVelocity) > 1.0f)
desiredVelocity *= 0.70710678f; // 1/sqrt(2)
//Skala enligt movespeed
desiredVelocity *= moveSpeed;
//Skapa en matris för att transformera velocity till samma koordinatsystem som desiredVel är angivet i
glm::mat4 mat = glm::gtc::matrix_transform::rotate(glm::mat4(1.0f),-cam->dir.z,glm::vec3(0.0f,0.0f,1.0f));
//Skapa en kopia på velocity och transformera denna.
glm::vec4 temp(velocity,1.0f);
temp = mat * temp;
//Öka på temp enligt desired
temp.x=Inc(desiredVelocity.x,temp.x,maxAcceleration);
temp.y=Inc(desiredVelocity.y,temp.y,maxAcceleration);
//Transformera tillbaks
mat = glm::gtc::matrix_transform::rotate(glm::mat4(1.0f),cam->dir.z,glm::vec3(0.0f,0.0f,1.0f));
temp = mat * temp;
velocity.x=temp.x;
velocity.y=temp.y;
if(!glfwGetKey(GLFW_KEY_SPACE))
jumpOk=true;
if(!airborne && jumpOk && glfwGetKey(GLFW_KEY_SPACE))
{
velocity.z=jumpStrength;
jumpOk=false;
}
velocity.z -= fallAcceleration;
airborne=true;
if(velocity.z<=0.0)
{
//AlignToGroundConvex();
}
matrix[3].x += velocity.x;
matrix[3].y += velocity.y;
matrix[3].z += velocity.z;
NewtonBodySetMatrix(body,&matrix[0][0]);
for(int i=0; i<5; i++)
{
glm::vec3 g_minBox(matrix[3]+this->minBox);
glm::vec3 g_maxBox(matrix[3]+this->maxBox);
minPush = maxPush = glm::vec3(0.0f);
NewtonWorldForEachBodyInAABBDo(world,&g_minBox[0],&g_maxBox[0],BodyIterator,this);
matrix[3].x += (minPush.x+maxPush.x);
matrix[3].y += (minPush.y+maxPush.y);
matrix[3].z += (minPush.z+maxPush.z);
NewtonBodySetMatrix(body,&matrix[0][0]);
glm::vec3 normal;
normal = minPush + maxPush;
if(glm::dot(normal,normal)>0.0f)
normal = glm::normalize(normal);
velocity = velocity - normal*glm::dot(normal,velocity);
}
if(!airborne)
velocity.z = std::min(velocity.z,0.0f);
}
void world::wake(const aabb_t &aabb) const
{
vec3 min = aabb.getMinimum(), max = aabb.getMaximum();
NewtonWorldForEachBodyInAABBDo(_world, &min.x, &max.x, &_wakeAABBCB, NULL);
}
