int CVMergeLines::Process(CVPipeline * pipe)
{
for (int i = 0; i < pipe->lines.Size(); ++i)
{
Line & line1 = pipe->lines[i];
// Make sure it's normalized.
Vector3f dir = line1.direction.NormalizedCopy();
for (int j = 0; j < pipe->lines.Size(); ++j)
{
if (i == j)
continue;
Line & line2 = pipe->lines[j];
// Check that the directions align.
Vector3f dir2 = line2.direction.NormalizedCopy();
if (AbsoluteValue(dir.DotProduct(dir2)) < 0.95f)
continue;
// Check overlap distance. If not overlapping at all, skip 'em.
// if (line1.stop.x < line2.start.x ||
// line1.start.x > line2.stop.x)
// continue;
// Check the distance between the lines.
float distance = (line1.start - line2.start).Length();
if (distance < maxDistance->GetFloat())
{
// Merge 'em
line1.Merge(line2);
pipe->lines.RemoveIndex(j);
--j;
}
}
}
returnType = CVReturnType::LINES;
return returnType;
}
/// Returns false if the colliding entities are no longer in contact after resolution.
bool FirstPersonCR::ResolveCollision(Collision & c)
{
Entity * dynamic;
Entity * other;
if (c.one->physics->type == PhysicsType::DYNAMIC)
{
dynamic = c.one;
other = c.two;
}
else
{
dynamic = c.two;
other = c.one;
}
if (c.one->physics->noCollisionResolutions || c.two->physics->noCollisionResolutions)
return false;
// Retardation?
// if (dynamic->physics->lastCollisionMs + dynamic->physics->minCollisionIntervalMs > physicsNowMs)
// return false;
dynamic->physics->lastCollisionMs = physicsNowMs;
Entity * dynamic2 = (other->physics->type == PhysicsType::DYNAMIC) ? other : NULL;
Entity * staticEntity;
Entity * kinematicEntity;
if (dynamic == c.one)
other = c.two;
else
other = c.one;
if (!dynamic2)
{
staticEntity = other->physics->type == PhysicsType::STATIC? other : NULL;
kinematicEntity = other->physics->type == PhysicsType::KINEMATIC? other : NULL;
}
// std::cout<<"\nCollision: "<<c.one->name<<" "<<c.two->name;
// Collision..!
// Static-Dynamic collision.
if (!dynamic2)
{
PhysicsProperty * pp = dynamic->physics;
// Skip? No.
// if (kinematicEntity)
// return true;
/// Flip normal if dynamic is two.
if (dynamic == c.two)
c.collisionNormal *= -1;
if (c.collisionNormal.y > 0.8f)
pp->lastGroundCollisionMs = physicsNowMs;
// Default plane? reflect velocity upward?
// Reflect based on the normal.
float velDotNormal = pp->velocity.DotProduct(c.collisionNormal);
/// This will be used to reflect it.
Vector3f velInNormalDir = c.collisionNormal * velDotNormal;
Vector3f velInTangent = pp->velocity - velInNormalDir;
Vector3f newVel = velInTangent * (1 - pp->friction) + velInNormalDir * (-pp->restitution);
/// Apply resitution and stuffs.
dynamic->physics->velocity = newVel;
assert(dynamic->parent == 0);
/// Adjusting local position may not help if child entity.
// Old code
Vector3f moveVec = AbsoluteValue(c.distanceIntoEachOther) * c.collisionNormal;
// if (moveVec.x || moveVec.z)
// std::cout<<"\nMoveVec: "<<moveVec;
dynamic->localPosition += moveVec;
/// For double-surface collision resolution (not bouncing through walls..)
/// For the previously backwards-collisions.
// if (c.distanceIntoEachOther < 0)
// dynamic->localPosition += c.distanceIntoEachOther * c.collisionNormal;
// else // Old code
// dynamic->localPosition += AbsoluteValue(c.distanceIntoEachOther) * c.collisionNormal;
/// If below threshold, sleep it.
if (dynamic->physics->velocity.Length() < inRestThreshold && c.collisionNormal.y > 0.8f)
{
// Sleep eeet.
dynamic->physics->state |= CollisionState::IN_REST;
// Nullify velocity.
dynamic->physics->velocity = Vector3f();
}
if (debug == 7)
{
std::cout<<"\nCollision resolution: "<<(c.one->name+" "+c.two->name+" ")<<c.collisionNormal<<" onePos"<<c.one->worldPosition<<" twoPos"<<c.two->worldPosition;
}
}
// Dynamic-dynamic collision.
else
{
// std::cout<<"\nProblem?" ;
/// Push both apart?
PhysicsProperty * pp = dynamic->physics, * pp2 = dynamic2->physics;
/// Flip normal if dynamic is two.
// See if general velocities align with one or the other? or...
// if (dynamic == c.two)
;// c.collisionNormal *= -1;
// std::cout<<"\nNormal: "<<c.collisionNormal;
// Default plane? reflect velocity upward?
/// This will be used to reflect it.
Vector3f velInNormalDir = c.collisionNormal * pp->velocity.DotProduct(c.collisionNormal);
Vector3f velInNormalDir2 = c.collisionNormal * pp2->velocity.DotProduct(c.collisionNormal);
Vector3f velInTangent = pp->velocity - velInNormalDir,
velInTangent2 = pp2->velocity - velInNormalDir2;
Vector3f velInTangentTot = velInTangent + velInTangent2,
velInNormalDirTot = velInNormalDir + velInNormalDir2;
/// Use lower value restitution of the two?
float restitution = 0.5f; // MinimumFloat(pp->restitution, pp2->restitution);
Vector3f normalDirPart = velInNormalDirTot * restitution * 0.5f;
Vector3f to2 = (dynamic2->worldPosition - dynamic->worldPosition).NormalizedCopy();
float partTo2 = normalDirPart.DotProduct(to2);
// std::cout<<" normalDirPart: "<<normalDirPart;
float normalDirPartVal = normalDirPart.Length();
Vector3f fromCollisionToDyn1 = (dynamic->worldPosition - c.collissionPoint).NormalizedCopy();
Vector3f fromCollisionToDyn2 = (dynamic2->worldPosition - c.collissionPoint).NormalizedCopy();
pp->velocity = velInTangent * (1 - pp->friction) + fromCollisionToDyn1 * normalDirPartVal;
pp2->velocity = velInTangent2 * (1 - pp->friction) + fromCollisionToDyn2 * normalDirPartVal;
/// Apply resitution and stuffs.
assert(dynamic->parent == 0);
/// Adjusting local position may not help if child entity.
// Old code
float distanceIntoAbsH = AbsoluteValue(c.distanceIntoEachOther * 0.5f);
dynamic->localPosition += distanceIntoAbsH * fromCollisionToDyn1;
dynamic2->localPosition += distanceIntoAbsH * fromCollisionToDyn2;
/// For double-surface collision resolution (not bouncing through walls..)
/// If below threshold, sleep it.
if (dynamic->physics->velocity.Length() < inRestThreshold && c.collisionNormal.y > 0.8f)
{
pp->Sleep();
}
else
pp->Activate();
if (pp2->velocity.Length() < inRestThreshold && c.collisionNormal.y > 0.8f)
{
pp2->Sleep();
}
else
pp2->Activate();
if (debug == 7)
{
std::cout<<"\nCollision resolution: "<<(c.one->name+" "+c.two->name+" ")<<c.collisionNormal<<" onePos"<<c.one->worldPosition<<" twoPos"<<c.two->worldPosition;
}
}
// Check collision normal.
return true;
}
/// For NormalMapping~
void Mesh::CalculateUVTangents()
{
bool defaultCalcUVTangents = false;
if (!defaultCalcUVTangents)
return;
// Not working anyway.. fit it.
if (this->uvs.Size() == 0)
{
std::cout<<"\nNo UVs found. Skipping.";
return;
}
for (int i = 0; i < faces.Size(); ++i)
{
MeshFace * f = &faces[i];
if (f->numVertices < 3)
continue;
// Reference:
// http://www.terathon.com/code/tangent.html
/// 3 Points on the faces
Vector3f v1, v2, v3;
v1 = vertices[f->vertices[0]];
v2 = vertices[f->vertices[1]];
v3 = vertices[f->vertices[2]];
Vector2f uv1, uv2, uv3;
uv1 = uvs[f->uvs[0]];
uv2 = uvs[f->uvs[1]];
uv3 = uvs[f->uvs[2]];
/// Diff-vectors.
Vector3f v1ToV2 = v2 - v1,
v1ToV3 = v3 - v1;
Vector2f uv1ToUV2 = uv2 - uv1,
uv1ToUV3 = uv3 - uv1;
Vector2f deltaUV1 = uv1ToUV2,
deltaUV2 = uv1ToUV3;
Vector3f deltaPos1 = v1ToV2;
Vector3f deltaPos2 = v1ToV3;
// Edges of the triangle : postion delta
float r = 1.0f / (deltaUV1.x * deltaUV2.y - deltaUV1.y * deltaUV2.x);
Vector3f tangent = (deltaPos1 * deltaUV2.y - deltaPos2 * deltaUV1.y)*r;
Vector3f biTangent = (deltaPos2 * deltaUV1.x - deltaPos1 * deltaUV2.x)*r;
/// Check handed-ness.
Vector3f crossProduct = normals[f->normals[0]].CrossProduct(tangent);
float dotProduct = crossProduct.DotProduct(biTangent);
// std::cout<<"\nHandednesS: "<<dotProduct > 0;
if (dotProduct < 0)
tangent *= -1;
/// Add the tanget and bitangnet
f->uvTangent = tangent;
f->uvBiTangent = biTangent;
/// Help http://www.opengl-tutorial.org/intermediate-tutorials/tutorial-13-normal-mapping/
}
}
void PhysicsManager::ApproximateIntegrate(Entity * entity, float timeSinceLastUpdate)
{
PhysicsProperty * physics = entity->physics;
// Calculate velocity using the moment?
physics->CalculateVelocity();
float currentVelocity = physics->velocity.Length();
float velocitySquared = currentVelocity * currentVelocity;
if (currentVelocity > 0.0001f)
{
entity->localPosition += physics->velocity * timeSinceLastUpdate;
physics->state |= CollisionState::COLLIDING;
physics->state &= ~CollisionState::IN_REST;
/// Only apply this if we're exceeding stuffs
/** http://en.wikipedia.org/wiki/Drag_%28physics%29
Drag depends on the properties of the fluid and on the size, shape,
and speed of the object. One way to express this is by means of the drag equation:
Fd = 0.5 * p * v^2 * Cd * A
where
FD is the drag force,
p is the density of the fluid,[12]
v is the speed of the object relative to the fluid,
A is the cross-sectional area, and
Cd is the drag coefficient – a dimensionless number.
The drag coefficient depends on the shape of the object and on the Reynolds number:
Re
*/
#define AIR_DENSITY 1.225 // kg/m^3
#define PT_VELOCITY dynamicEntity->physics->velocity
#define MASS 500
if (currentVelocity > 10.f)
{
float dragCoefficient = 0.001f;
float crossSectionalArea = physics->physicalRadius;
float dragForce = 0.5f * airDensity * velocitySquared * dragCoefficient * crossSectionalArea;
float dragEnergy = dragForce * timeSinceLastUpdate;
float velocityDecrease = sqrt(dragEnergy / MASS * 2);
// And decrement the velocity.
float preVel = physics->velocity.Length();
Vector3f decrease = - physics->velocity.NormalizedCopy() * velocityDecrease;
physics->velocity += decrease;
float postVel = physics->velocity.Length();
// This looks like it should always be true,
// but it's false if x is a NaN.
if (!(physics->velocity[0] == physics->velocity[0])){
// std::cout<<"\nwosh-";
}
}
}
#ifdef _DEBUG
// Make object bounce up again if at -100.0f made for testing physics only
/* if (dynamicEntity[i]->position[1] < -100.0f && dynamicEntity[i]->physics->velocity[1] < 0){
dynamicEntity[i]->position[1] = -100.0f;
dynamicEntity[i]->physics->velocity[1] = dynamicEntity[i]->physics->velocity[1] * -0.4;
}
*/
#endif
// Add gravitation and acceleration to velocities
if (!(physics->state & CollisionState::IN_REST))
{
// dynamicEntity->physics->velocity += this->gravitation * timeSinceLastUpdate;
physics->linearMomentum += this->gravitation * physics->mass * timeSinceLastUpdate * physics->gravityMultiplier;
// std::cout<<"Applying gravitation to velocity.";
}
// Always apply acceleration unless otherwise noted.
if (physics->acceleration.MaxPart() > ZERO)
{
Vector3f velIncrease = entity->rotationMatrix.Product(physics->acceleration);
velIncrease *= timeSinceLastUpdate;
Vector3f momentumIncrease = velIncrease * physics->mass;
physics->linearMomentum += momentumIncrease;
// std::cout<<"Accelerating velocityIncrease: "<<velocityIncrease<<" newVel: "<<dynamicEntity->physics->velocity;
physics->state |= CollisionState::COLLIDING;
physics->state &= ~CollisionState::IN_REST;
}
/// Apply angular velocities
float angularVelocitySquared = physics->angularVelocity.LengthSquared();
float angularVelocity = sqrt(angularVelocitySquared);
if (angularVelocitySquared > 0.000001f)
{
Vector3f lookAtPreRotate = entity->rotationMatrix * Vector4d(0,0,-1,1);
if (physics->useQuaternions)
{
///Calculate with quaternions.
PhysicsProperty * physics = entity->physics;
Quaternion & orientation = physics->orientation;
if (physics->angularVelocity.MaxPart() > 0){
// std::cout<<"\nQuaternion, pre: "<<physics->orientation;
// std::cout<<"\nTime since last update: "<<timeSinceLastUpdate;
physics->orientation.ApplyAngularVelocity(physics->angularVelocity, timeSinceLastUpdate);
physics->orientation.Normalize();
// std::cout<<" post: "<<physics->orientation;
// std::cout<<"\nAngular Velocity: "<<physics->angularVelocity;
}
}
// Euclidean.
else {
/// Default gimbal-locking rotations
entity->Rotate(physics->angularVelocity * timeSinceLastUpdate);
}
// Recalculate the matrix?
entity->RecalculateMatrix();
/// Adjust velocity in the movement direction depending on our new rotation!
Vector3f lookAtPostRotate = entity->rotationMatrix * Vector4d(0,0,-1,1);
float velDotLookatPreRotate = lookAtPreRotate.DotProduct(physics->velocity);
float velPreRotate = physics->velocity.Length();
Vector3f oldVelocity = physics->velocity;
Vector3f velocityMinusLocalZVelocity = oldVelocity - lookAtPreRotate * velDotLookatPreRotate;
/// Change it.
physics->velocity = velocityMinusLocalZVelocity +
velDotLookatPreRotate * lookAtPostRotate * physics->velocityRetainedWhileRotating +
velDotLookatPreRotate * lookAtPreRotate * (1.0f - physics->velocityRetainedWhileRotating);
/// And apply some damping or it will increase.
float dampingDueToRotation = pow(0.99754f, angularVelocity);
// Close, 0.99854f
physics->velocity *= dampingDueToRotation;
physics->velocity *= pow(linearDamping, timeSinceLastUpdate);
// if (dampingDueToRotation < 0.9985f)
// std::cout<<"\nDamping due to rotation: "<<dampingDueToRotation;
float velPostRotate = physics->velocity.Length();
// assert(velPostRotate <= velPreRotate);
// std::cout<<"\nUpdating velocity using rotations..? "<<oldVelocity<<" newVel: "<<dynamicEntity->physics->velocity;
// Decrease the velocity as if we've got some air in the way too?
physics->angularVelocity *= pow(angularDamping, timeSinceLastUpdate);
physics->state |= CollisionState::COLLIDING;
physics->state &= ~CollisionState::IN_REST;
}
// Apply angular acceleration
if (physics->angularAcceleration.MaxPart() > ZERO){
// Screw the quaternions and stuff for now... do it all in local space
Vector3f angularVelocityIncrease = physics->angularAcceleration * timeSinceLastUpdate;
physics->angularVelocity += angularVelocityIncrease;
physics->state |= CollisionState::COLLIDING;
physics->state &= ~CollisionState::IN_REST;
}
// Apply linear damping
physics->linearMomentum *= pow(physics->linearDamping, timeSinceLastUpdate);
// Update velocity.
if (physics->inverseMass == 0)
physics->inverseMass = 1 / physics->mass;
physics->velocity = physics->linearMomentum * physics->inverseMass;
entity->localPosition += physics->velocity * timeSinceLastUpdate;
// Move position
// Reposition them in the entityCollisionOctree as needed...!
// This could maybe be done less times per loop, but that would apply to the whole loop below if so...!
}
/** All entities sent here should be fully dynamic!
Kinematic ones may or may not work (consider adding own integration function).
*/
void SRIntegrator::IntegrateDynamicEntities(List<Entity*> & dynamicEntities, float timeInSeconds)
{
FirstPersonIntegrator::IntegrateDynamicEntities(dynamicEntities, timeInSeconds);
for (int i = 0; i < dynamicEntities.Size(); ++i)
{
// check for magnetics.
Entity * entity = dynamicEntities[i];
if (entity->physics->state & CollisionState::IN_REST)
continue;
MagneticProperty * magProp = entity->GetProperty<MagneticProperty>();
if (magProp)
{
// Rotate a bit.
// Raycast to floor on location, at forward + 1 and side + 1, then rotate so that the ship is flat to the level.
Vector3f upVec = entity->rotationMatrix.GetColumn(1),
forwardVec = -entity->rotationMatrix.GetColumn(2),
rightVec = entity->rotationMatrix.GetColumn(0);
/// New rolling, check nearest points of track.
TrackPoint * tp = track->NearestPoint(entity->worldPosition);
if (!tp || tp->next == 0)
continue;
TrackPoint * next = tp->next->next;
if (!next)
continue;
/// Desired forward, and up-right vectors of the track.
Vector3f desForward = (next->pos - tp->pos).NormalizedCopy(),
desUp = tp->up,
desRight = tp->right;
float desForwardDotCurrForward = desForward.NormalizedCopy().DotProduct(forwardVec);
Vector3f desForwardInCurrDir = desForwardDotCurrForward * desForward; // Less than 1
float forwardDotDesUp = forwardVec.DotProduct(desUp);
// Pitch down or up?
Vector3f forwardProjected = desForwardDotCurrForward * desForward;
Vector3f diffForwardCurrForward = forwardVec - desForward;
/// Pitch needed?
float pitchNeeded = diffForwardCurrForward.DotProduct(upVec);
// if (AbsoluteValue(pitchNeeded) > 0.1f)
// std::cout<<"\nPitch needed: "<<pitchNeeded;
if (desForwardDotCurrForward > 0)
pitchNeeded *= -1;
// Rotate slightly around side-vector.
entity->physics->angularVelocity -= rightVec * pitchNeeded * timeInSeconds * 0.2f;
/// Barrel-roll.
Vector3f diffUpCurrUp = upVec - desUp;
float barrelNeeded = diffUpCurrUp.DotProduct(rightVec);
// if (AbsoluteValue(barrelNeeded) > 0.1f)
// std::cout<<"\nBarrel needed: "<<barrelNeeded;
entity->physics->angularVelocity += forwardVec * barrelNeeded * timeInSeconds * 0.1f;
/// Add some velocity towards the next and previous node, acting as magnetism down towards the field?
entity->physics->velocity -= desUp * timeInSeconds * 5.f + (entity->worldPosition - tp->pos).NormalizedCopy() * 5.f * timeInSeconds;
/// Old rolling
/*
Ray ray(entity->worldPosition, -upVec); // 0 - side, 1 - up, 2 - forward
ray.collisionFilter = CC_TRACK;
List<Intersection> iSecs = Physics.Raycast(ray);
float distBelow = iSecs.Size()? iSecs[0].distance : 0.f;
// Ray again.
ray.start += forwardVec;
iSecs = PhysicsMan.Raycast(ray);
float distInFront = iSecs.Size()? iSecs[0].distance : 0.f;
ray.start = entity->worldPosition + rightVec;
iSecs = PhysicsMan.Raycast(ray);
float distSide = iSecs.Size()? iSecs[0].distance : 0.f;
float distPitch = distInFront - distBelow;
float distRoll = distSide - distBelow;
// Rotate accordingly for pitch.
if (distInFront && distBelow && AbsoluteValue(distPitch) < 2.f && distBelow < 5.f)
{
// Rotate slightly around side-vector.
entity->physics->angularVelocity += rightVec * distPitch * timeInSeconds * 0.2f;
}
// Rotate accordingly for roll.
if (distSide && distBelow && AbsoluteValue(distRoll) < 2.f && distBelow < 5.f)
{
// Rotate slightly around side-vector.
entity->physics->angularVelocity -= forwardVec * distRoll * timeInSeconds * 0.1f;
}
/// If not above anything, rotate to level-plane.
if (distBelow == 0)
{
magProp->timeOutsideTrack += timeInSeconds;
if (magProp->timeOutsideTrack > 2.f)
{
/// If nothing detected, rotate so that the ship will be level again, by rolling?
if (forwardVec.y > 0.2f)
{
entity->physics->angularVelocity += forwardVec.y * rightVec * timeInSeconds * 0.1f;
}
/// Right-vec not aligned with XZ plane? Roll until it is.
if (AbsoluteValue(rightVec.y) > 0.2f)
{
entity->physics->angularVelocity += rightVec.y * forwardVec * timeInSeconds * 0.1f;
}
if (upVec.y < 0.8f)
{
// Barrel-roll up.
entity->physics->angularVelocity += (-1.f + upVec.y) * forwardVec * timeInSeconds * 0.1f;
}
}
}
else
{
magProp->timeOutsideTrack = 0;
}
*/
// Same for roll.
// If not close to a surface... rotate to.. stuff?
// entity->physics->angularVelocity.x = 0.00f;
}
}
}
