void TmxMap::Update(float dt)
{
m_prevPos = myPosition;
//CGameObject::Update(dt);
ResolveCollision();
// update the position of the map
myPosition += myVelocity;
// update the postion of the first tile to be drawn
int x = (int)myPosition.x % tileWidth + (myPosition.x > 0 ? -tileWidth : 0);
if (abs(x) == tileWidth)
{
x = 0;
}
m_firstTilePos.x = x + tileWidth / 2 + 0.0f;
m_firstTilePos.y = (int)myPosition.y % tileHeight + 0.0f + tileHeight / 2;
// update the row of the first tile to be drawn
m_tileRow = (m_pixelHeight / 2 - (int)myPosition.y) / tileHeight;
// update the column of the first tile to be drawn
m_tileColumn = (m_pixelWidth / 2 - (int)myPosition.x) / tileWidth;
myVelocity.x = 0.f;
}
void Contact::Solve()
{
if (m_t < 0.0f)
ResolveOverlap();
ResolveCollision();
}
void dCustomPlayerController::PreUpdate(dFloat timestep)
{
dFloat timeLeft = timestep;
const dFloat timeEpsilon = timestep * (1.0f / 16.0f);
m_impulse = dVector(0.0f);
m_manager->ApplyMove(this, timestep);
//SetForwardSpeed(1.0f);
//SetLateralSpeed(0.0f);
//SetHeadingAngle(45.0f*dDegreeToRad);
// set player orientation
dMatrix matrix(dYawMatrix(GetHeadingAngle()));
NewtonBodyGetPosition(m_kinematicBody, &matrix.m_posit[0]);
NewtonBodySetMatrix(m_kinematicBody, &matrix[0][0]);
// set play desired velocity
dVector veloc(GetVelocity() + m_impulse.Scale(m_invMass));
NewtonBodySetVelocity(m_kinematicBody, &veloc[0]);
// determine if player has to step over obstacles lower than step hight
ResolveStep(timestep);
// advance player until it hit a collision point, until there is not more time left
for (int i = 0; (i < D_DESCRETE_MOTION_STEPS) && (timeLeft > timeEpsilon); i++) {
if (timeLeft > timeEpsilon) {
ResolveCollision();
}
dFloat predicetdTime = PredictTimestep(timestep);
NewtonBodyIntegrateVelocity(m_kinematicBody, predicetdTime);
timeLeft -= predicetdTime;
}
}
void Contact::ResolveCollision()
{
if (!m_pxBodies[0] || !m_pxBodies[1])
return;
for(int i = 0; i < m_iNumContacts; i ++)
{
ResolveCollision(m_xContacts[i][0], m_xContacts[i][1]);
}
}
void DContact::Solve()
{
// since we don't support time-to-collision, this will always be true
if(t<0.0f)
{
// resolve overlaps
for(u32 i=0;i<numDContacts;++i)
ResolveOverlap(DContacts[i][0], DContacts[i][1]);
}
// resolve collisions
for(u32 i=0;i<numDContacts;++i)
ResolveCollision(DContacts[i][0], DContacts[i][1]);
};
void ECS_CalculateCollision(ECS_Entity* entities, size_t count)
{
if(entities == NULL) return;
size_t i, j;
for(i = 0; i < count - 1; i++)
{
ECS_Entity* a = entities + i;
if((a->mask & ECS_SYSTEM_COLLISION) != ECS_SYSTEM_COLLISION) continue;
for(j = i + 1; j < count; j++)
{
ECS_Entity* b = entities + j;
if((b->mask & ECS_SYSTEM_COLLISION) != ECS_SYSTEM_COLLISION) continue;
ResolveCollision(a, b);
}
}
}
void CollisionSystem::ResolveCollisions()
{
for (size_t i = 0; i < m_buckets.size(); ++i)
{
std::list<Actor*>& units = m_buckets[i];
for(auto outUnit = units.begin(); outUnit != units.end(); ++outUnit)
{
auto inUnit = outUnit;
++inUnit;
for (inUnit; inUnit != units.end(); ++inUnit)
{
ResolveCollision((*outUnit), (*inUnit));
}
}
}
}
////////////////////////////////////////////////////////////////
// ResolveCollision
////////////////////////////////////////////////////////////////
// calculates the change in angular and linear velocity of two
// colliding objects
////////////////////////////////////////////////////////////////
// parameters :
// ------------
// Ncoll : normal of collision
// t : time of collision, (if negative, it's a penetration depth)
// fCoF : coefficient of friction
// fCoR : coefficient of restitution
// C0 : point of contact on object 0
// P0 : centre of gravity (position) of object 0
// V0 : linear Velocity of object 0
// w0 : angular velocity of object 0
// m0 : inverse mass of object 0
// i0 : inverse inertia of object 0
// C1 : point of contact on object 1
// P1 : centre of gravity (position) of object 1
// V1 : linear Velocity of object 1
// w1 : angular velocity of object 1
// m1 : inverse mass of object 1
// i1 : inverse inertia of object 1
//
// return values : V0, w0, V1, w1 will change upon a collision
// -------------
///////////////////////////////////////////////////////////////
void ResolveCollision(const glm::vec2& Ncoll, float t, float fCoF, float fCoR,
const glm::vec2& C0, const glm::vec2& P0, glm::vec2& V0, float& w0, float m0, float i0,
const glm::vec2& C1, const glm::vec2& P1, glm::vec2& V1, float& w1, float m1, float i1)
{
//------------------------------------------------------------------------------------------------------
// pre-computations
//------------------------------------------------------------------------------------------------------
float tcoll = (t > 0.0f)? t : 0.0f;
glm::vec2 Q0 = P0 + V0 * tcoll;
glm::vec2 Q1 = P1 + V1 * tcoll;
glm::vec2 R0 = C0 - Q0;
glm::vec2 R1 = C1 - Q1;
glm::vec2 T0 = glm::vec2(-R0.y, R0.x);
glm::vec2 T1 = glm::vec2(-R1.y, R1.x);
glm::vec2 VP0 = V0 - T0 * w0; // point velocity (SIGN IS WRONG)
glm::vec2 VP1 = V1 - T1 * w1; // point velocity (SIGN IS WRONG)
//------------------------------------------------------------------------------------------------------
// impact velocity
//------------------------------------------------------------------------------------------------------
glm::vec2 Vcoll = VP0 - VP1;
float vn = glm::dot(Vcoll, Ncoll);
glm::vec2 Vn = vn * Ncoll;
glm::vec2 Vt = Vcoll - Vn;
// separation
if (vn > 0.0f)
return;
float vt = glm::length(Vt);
//------------------------------------------------------------------------------------------------------
// compute impulse (frction and restitution).
// ------------------------------------------
//
// -(1+Cor)(Vel.norm)
// j = ------------------------------------------------------------
// [1/Ma + 1/Mb] + [Ia' * (ra x norm)²] + [Ib' * (rb x norm)²]
//------------------------------------------------------------------------------------------------------
glm::vec2 J;
glm::vec2 Jt(0.0f, 0.0f);
glm::vec2 Jn(0.0f, 0.0f);
float t0 = cross(R0, Ncoll) * cross(R0, Ncoll) * i0;
float t1 = cross(R1, Ncoll) * cross(R1, Ncoll) * i1;
float m = m0 + m1;
float denom = m + t0 + t1;
float jn = vn / denom;
Jn = (-(1.0f + fCoR) * jn) * Ncoll;
if (dbg_UseFriction)
{
Jt = (fCoF * jn) * glm::normalize(Vt);
}
J = Jn + Jt;
//------------------------------------------------------------------------------------------------------
// changes in momentum
//------------------------------------------------------------------------------------------------------
glm::vec2 dV0 = J * m0;
glm::vec2 dV1 =-J * m1;
float dw0 =-cross(R0, J) * i0; // (SIGN IS WRONG)
float dw1 = cross(R1, J) * i1; // (SIGN IS WRONG)
//------------------------------------------------------------------------------------------------------
// apply changes in momentum
//------------------------------------------------------------------------------------------------------
if (m0 > 0.0f) V0 += dV0;
if (m1 > 0.0f) V1 += dV1;
if (m0 > 0.0f) w0 += dw0;
if (m1 > 0.0f) w1 += dw1;
//------------------------------------------------------------------------------------------------------
// Check for static frcition
//------------------------------------------------------------------------------------------------------
if (vn < 0.0f && fCoF > 0.0f)
{
float cone = -vt / vn;
if (cone < fCoF)
{
// treat static friciton as a collision at the contact point
glm::vec2 Nfriction = glm::normalize(-Vt);
float fCoS = GetStaticFriction;
ResolveCollision(Nfriction, 0.0f, 0.0f, fCoS,
C0, P0, V0, w0, m0, i0,
C1, P1, V1, w1, m1, i1);
}
}
}
void RuleStrongDistance::ResolveStrongDistance(const CircleObjectPtr& a_Object1, const CircleObjectPtr& a_Object2, const BarProperties& a_Properties)
{
ResolveCollision(a_Object1, a_Object2, a_Properties.Distance, 0.00, false, false);
}
void RuleStrongBar::ResolveStrongBar(const CircleObjectPtr& a_Object1, const CircleObjectPtr& a_Object2, const BarProperties& a_Properties)
{
ResolveCollision(a_Object1, a_Object2, a_Properties.Distance, 0.00, true, true);
}
void RuleContact::ResolveContact(const CircleObjectPtr& a_Object1, const CircleObjectPtr& a_Object2)
{
ResolveCollision(a_Object1, a_Object2, a_Object1->Radius + a_Object2->Radius, 0.001, false, true);
}
