void PhysicalPlayer::applyAcceleration(double acceleration) {
if(!onGround) return;
rigidBody->activate();
double constant = GET_SETTING("physics.constant.accel", 1.0);
double brakeConstant = GET_SETTING("physics.constant.brake", 5.0);
btTransform transform = rigidBody->getWorldTransform();
btMatrix3x3 matrix(transform.getRotation());
Math::Point orientation = Converter::toPoint(matrix
* Converter::toVector(Math::Point(0.0, 0.0, 1.0) * constant));
//LOG(PHYSICS, "accel at " << Misc::Sleeper::getTimeMilliseconds());
if(getSliding()) {
constant *= GET_SETTING("physics.slipstate.accelfactor", 1.0);
}
if (acceleration >= 0.0 || getLinearVelocity().dotProduct(orientation) < 0.0) {
double paintInfluence = (speedBoost - 1.0)
* GET_SETTING("game.paint.boostinfluence", 1.0) + 1.0;
applyForce(orientation * constant * acceleration * paintInfluence * traction);
}
else {
applyForce(orientation * brakeConstant * acceleration * traction);
}
updatePhysicalInfo();
}
//--------------------------------------------------------------
void ofApp::update(){
wind[0] = ofNoise(wind[0])/100 * -1;
applyForce(wind);
// bounce from top
if (loc[1] < 59) {
loc[1] = 60;
vel[1] = 0;
applyForce(ceiling);
} else if (loc[0] < 60) {
loc[0] = 60;
}
vel += acc;
loc += vel;
acc.scale(0);
// arduino
// receive and analyse data in an infinite while loop
while (true) {
int c = serial.readByte();
// break the loop if all data received by the serial port was already processed or there is an error
if (c == OF_SERIAL_NO_DATA || c == OF_SERIAL_ERROR || c == 0)
break;
// if a new line symbol is received, set a value to the arduinoSpeed variable
// clear the buffer to be ready to receive the next value
if (c == '\n') {
arduinoSpeed = ofToFloat(str);
str = "";
}
// append the received value to the buffer
else str.push_back(c);
}
}
void StrainForce::solve(double time) {
if (nodes.size() < 1)
return;
if (nodes.size() < 2) {
if (auto sys = system.lock()) {
auto node = sys->getNode(nodes.front());
node->applyForce(appliedForce);
return;
}
}
if (!initialized) {
setup();
initialized = true;
}
if (auto sys = system.lock()) {
size_t size = nodes.size()-1;
auto mainNode = sys->getNode(mainId);
mainNode->applyForce(appliedForce);
for (size_t i = 0; i < size; ++i) {
auto node = sys->getNode(otherIds[i]);
auto right = mainNode->getForce() - mainNode->getMass() / node->getMass() * node->getForce();
gsl_vector_set(vecB[0], i, right.x);
gsl_vector_set(vecB[1], i, right.y);
gsl_vector_set(vecB[2], i, right.z);
}
for (int i = 0; i < 3; ++i)
gsl_linalg_LU_solve(matA, permutation, vecB[i], vecX[i]);
for (size_t i = 0; i < size; ++i) {
auto node = sys->getNode(otherIds[i]);
glm::dvec3 f = {
gsl_vector_get(vecX[0], i),
gsl_vector_get(vecX[1], i),
gsl_vector_get(vecX[2], i),
};
node->applyForce(f);
mainNode->applyForce(-f);
}
if(fixedAxe)
for (size_t i = 0; i < nodes.size(); ++i)
{
auto node = sys->getNode(nodes[i]);
node->FixDirection(appliedForce);
}
}
}
void Boid::flock(vector<Boid> boids){
ofVec2f sep = separate(boids);
ofVec2f ali = align(boids);
ofVec2f coh = cohesion(boids);
sep = sep * separateForce;
ali = ali * alignForce;
coh = coh * cohesionForce;
applyForce(sep);
applyForce(ali);
applyForce(coh);
}
void Bird::flock(QVector<Bird> birds)
{
QVector3D separation = separate(birds);
QVector3D alignment = align(birds);
QVector3D cohesion = cohes(birds);
separation *= 2.5;
alignment *= 1.0f;
cohesion *= 1.0f;
applyForce(separation);
applyForce(alignment);
applyForce(cohesion);
}
// Applies the three laws to the flock of boids
void Boid::flock(vector<Boid> v)
{
Pvector sep = Separation(v);
Pvector ali = Alignment(v);
Pvector coh = Cohesion(v);
// Arbitrarily weight these forces
sep.mulScalar(SepW);
ali.mulScalar(AliW); // Might need to alter weights for different characteristics
coh.mulScalar(CohW);
// Add the force vectors to acceleration
applyForce(sep);
applyForce(ali);
applyForce(coh);
}
void PhysicsBody::updateMass(float oldMass, float newMass)
{
if (_dynamic && !_gravityEnabled && _world != nullptr && oldMass != PHYSICS_INFINITY)
{
applyForce(_world->getGravity() * oldMass);
}
cpBodySetMass(_info->getBody(), newMass);
if (_dynamic && !_gravityEnabled && _world != nullptr && newMass != PHYSICS_INFINITY)
{
applyForce(-_world->getGravity() * newMass);
}
}
void SpringForce::solve(double time) {
if (auto sys = system.lock()) {
auto firstNode = sys->getNode(first);
auto secondNode = sys->getNode(second);
glm::dvec3 p = firstNode->getPosition() - secondNode->getPosition();
glm::dvec3 f = stiffness * (glm::length(p) - length) * p/glm::length(p);
firstNode->applyForce(-f);
secondNode->applyForce(f);
}
}
void CMassPoint::update()
{
applyForce(Vector3D(0,0,Gravity * mass));
if (pos.z <= GroundHeight)
{
Vector3D v;
v = vel;
v.z = 0;
if (vel.z < 0)
{
v = vel;
v.x = 0;
v.y = 0;
applyForce(-v * GroundAbsorptionConstant);
applyForce(Vector3D(0, 0, GroundRepulsionConstant) * (GroundHeight - pos.z));
}
}
double r = sqrt(pos.x*pos.x + pos.y*pos.y);
if ( r >= 10)
{
meet_obstacle++;
if(meet_obstacle>5)
{
//direction *= -1;
meet_obstacle = 0;
}
Vector3D v;
Vector3D rad_vect = pos;
rad_vect.unitize();
v = vel;
v.z = 0;
double radial_component = v*rad_vect;
if (radial_component > 0)
{
v = -pos;
v.z = 0;
applyForce( v * GroundRepulsionConstant * (r-10));
}
}
applyForce(-vel * AirFrictionCoefficient);
//Air friction added
}
//===========================================
// Box2dPhysics::applyForce
//===========================================
void Box2dPhysics::applyForce(const Vec2f& force) {
if (!m_init)
throw PhysicsException("Instance of Box2dPhysics is not initialised", __FILE__, __LINE__);
applyForce(force, Vec2f(m_body->GetWorldCenter().x * m_worldUnitsPerMetre,
m_body->GetWorldCenter().y * m_worldUnitsPerMetre));
}
void p_Particle::update(float dt)
{
applyForce(gravity, dt);
stepPosition(dt);
time += dt;
}
//! applies external forces [e.g. gravity]
void simulation::AerodynamicBody::_computeExternal(const AerodynamicEnvironment &env) {
// model gravity
const float g = 9.88;
float weight = g * dynamicState.rigidBody.mass;
applyForce(math::matrix3x1<float>(0, -weight, 0), AerodynamicForce::External_force);
}
void Ball::collide(const Vector4& point)
{
Vector4 force;
position = point;
force = -(velocity).normalize() * 100;
applyForce(force);
}
void VerletParticle::update() {
if (!isLocked) {
applyBehaviors();
applyForce();
applyConstraints();
}
}
void PhysicsWorld::update(Scene * scene, unsigned dt){
/*std::cout << "<---- physics update ----> \n";
std::cout << "<---- collisons loop ----> \n";*/
/*std::cout << "i= "<< i << " | id = " << objectList[i]->getId() << " \n";
std::cout << "pos: (" << objectList[i]->getPosition().x << ", " << objectList[i]->getPosition().y << " )\n";
std::cout << "rigid body pos: (" << objectList[i]->getRigidBody()->getPosition().x << ", " << objectList[i]->getRigidBody()->getPosition().y << " )\n";
std::cout << "width: " << objectList[i]->getWidth() << "| height: " << objectList[i]->getHeight() << "\n";*/
this->collisions.clear();
scene->getCollisionLayer()->foreachPair(dt, [=](CollisionObject * a, CollisionObject * b, unsigned dt) {
glm::vec2 mtv = a->getCollisionShape()->checkCollision(b->getCollisionShape());
if(mtv != glm::vec2()){
this->collisions.push_back(CollisionEvent(a,b,mtv));
this->resolveCollision(a,b,mtv);
}
});
scene->getCollisionLayer()->foreachObject(dt, [=](CollisionObject * collisionObject, unsigned dt) {
if(!(collisionObject->getCollisionShape()->getIsStatic())) {
collisionObject->getCollisionShape()->getRigidBody()->setForce(glm::vec2());
applyForce(collisionObject->getCollisionShape()->getRigidBody() ,this->gravity);
collisionObject->step(dt);
}
});
}
// Start performing a jump.
void Character::jump(void)
{
if ( (action[JUMP] || action[FALL]) && !has_double_jumped) {
stopAction(JUMP);
stopAction(FALL);
has_double_jumped = true;
action[JUMP] = true;
applyForce(physics::vector3(Ogre::Vector3(0,jump_force,0)));
} else if (on_floor) {
if (!action[ATTACK] && !action[DEFEND] && !action[LAND]) {
stopAction(IDLE);
action[JUMP] = true;
applyForce(physics::vector3(Ogre::Vector3(0,jump_force,0)));
}
}
}
bool RigidBody::AABBvsPLANE(RigidBody* actor)
{
vec3 collisionNormal = actor->GetNormal();
float halflength = m_length / 2;
float halfheight = m_height / 2;
float halfwidth = m_width / 2;
vec3 Points[8];
vec3 pos = m_position;
Points[0] = vec3(pos.x - halflength, pos.y - halfheight, pos.z);
Points[1] = vec3(pos.x + halflength, pos.y - halfheight, pos.z);
Points[2] = vec3(pos.x, pos.y - halfheight, pos.z - halfwidth);
Points[3] = vec3(pos.x, pos.y - halfheight, pos.z + halfwidth);
Points[4] = vec3(pos.x - halflength, pos.y + halfheight, pos.z);
Points[5] = vec3(pos.x + halflength, pos.y + halfheight, pos.z);
Points[6] = vec3(pos.x, pos.y + halfheight, pos.z - halfwidth);
Points[7] = vec3(pos.x, pos.y + halfheight, pos.z + halfwidth);
for (unsigned int i = 0; i < 7; i++)
{
float AABBToPlane = glm::dot(Points[i], collisionNormal);
if (AABBToPlane < 0)
{
//If we are behind the plane then we flip the normal
collisionNormal *= -1;
AABBToPlane *= -1;
}
float halfheight = m_height / 2;
float intersection = halfheight - AABBToPlane;
if (intersection >= 0)
{
glm::vec3 planeNormal = actor->GetNormal();
if (AABBToPlane < 0)
{
planeNormal *= -1; //flip the normal if we are behind the plane
}
glm::vec3 forceVector = planeNormal * -1 * (glm::dot(planeNormal, GetVelocity()));
applyForce(1 * forceVector);
m_position += actor->GetNormal() * intersection * .5f;
//set cube velocity to zero here:
//std::cout << "Collision with plane has occured" << "\n";
return true;
}
}
return false;
}
void Vehicle::seek(ofVec2f target) {
ofVec2f desired = target - location;
desired.normalize();
desired *= maxspeed;
ofVec2f steer = desired - velocity;
steer.limit(maxforce);
applyForce(steer);
}
void Ball::collide(const Wall& wall){
Vector4 force;
double proj = (position - wall.center).dot(wall.normal);
position -= wall.normal * proj;
velocity -= wall.normal * proj * 150;
force = Vector4(rand() % 5, rand() % 5, rand() % 5);
applyForce(force);
}
//--------------------------------------------------------------
void testApp::update(){
if(draggable){
return;
}
ofVec2f force = handle - anchor;
float len = force.length();
float stretchLen = len - restLength;
force.normalize();
force *= -1*k*stretchLen;
applyForce(force);
applyForce(ofVec2f(0,gravity));
vel += acc;
vel *= 0.95;
handle += vel;
acc.set(0);
}
void PhysicsBody::setGravityEnable(bool enable)
{
if (_gravityEnabled != enable)
{
_gravityEnabled = enable;
if (_world != nullptr)
{
if (enable)
{
applyForce(_world->getGravity() * _mass);
}else
{
applyForce(-_world->getGravity() * _mass);
}
}
}
}
void PhysicalPlayer::applyTurning(double amount) {
if(!onGround) return;
rigidBody->activate();
double constant = GET_SETTING("physics.constant.turn", 1.0);
double centripetalConstant
= GET_SETTING("physics.constant.centripetal", 1.0);
//double leanConstant = GET_SETTING("physics.constant.lean", 1.0);
Math::Matrix matrix = getTransformation();
Math::Point forwardAxis = matrix * Math::Point(0.0, 0.0, 1.0, 0.0);
Math::Point centripetalAxis = matrix * Math::Point(-1.0, 0.0, 0.0, 0.0);
forwardAxis.normalize();
centripetalAxis.normalize();
double speed = getLinearVelocity().length();
#if 0
// turn in the opposite direction when travelling backwards
if (getLinearVelocity().dotProduct(forwardAxis) < 0) {
speed = -speed;
}
#endif
double speedFactor = GET_SETTING("physics.turning.speedfactor", 0.5);
double speedThreshhold = GET_SETTING("physics.turning.speedthreshhold", 15.0);
double falloffFactor = GET_SETTING("physics.turning.fallofffactor", 0.25);
double turning_factor = GET_SETTING("physics.turning.constant", 1.0);
if (speed <= speedThreshhold) {
turning_factor += sqrt(speed/speedThreshhold)*(speed*speedFactor);
}
else {
turning_factor += (speedThreshhold*speedFactor)*(1.0/(1.0+(speed-speedThreshhold)*falloffFactor));
}
// turn in the opposite direction when travelling backwards
if (getLinearVelocity().dotProduct(forwardAxis) < 0) {
turning_factor = -turning_factor;
}
if(getSliding()) {
centripetalConstant *= GET_SETTING("physics.slipstate.centripetalfactor", 1.0);
constant *= GET_SETTING("physics.slipstate.turnfactor", 1.0);
}
applyForce(centripetalAxis * centripetalConstant * turning_factor * amount);
applyTorque(-getUpDirection() * constant * turning_factor * amount);
// twist the car in response to a sharp turn
//applyTorque(getFrontDirection() * leanConstant * turning_factor * amount);
updatePhysicalInfo();
}
void Vehicle::seek() {
PVector *target = player->getLocation();
PVector *desired = PVector::sub(target, location);
desired->normalize();
desired->mult(maxspeed);
PVector *steer = PVector::sub(desired, velocity);
steer->limit(maxforce);
applyForce(steer);
}
void TorsionSpringForce::solve(double time) {
if (auto sys = system.lock()) {
auto leftNode = sys->getNode(left);
auto centerNode = sys->getNode(center);
auto rightNode = sys->getNode(right);
glm::dvec3 p1 = leftNode->getPosition() - centerNode->getPosition();
glm::dvec3 p2 = rightNode->getPosition() - centerNode->getPosition();
double cos_theta = glm::dot(p1,p2) / glm::length(p1) / glm::length(p2);
double theta;
// провека на nan
if(glm::abs(cos_theta) <= 1)
theta = ::acos(cos_theta); // всегда в пределах [0;pi]
else
theta = M_PI;
glm::dvec3 a;
glm::dvec3 b;
glm::dvec3 axb; // первое векторное произведение p1 и p2
glm::dvec3 c1; // первое двойное векторное произведение
glm::dvec3 c2; // второе двойное векторное произведение
double m = -stiffness * (defaultAngle - theta); // момент пружины
axb = glm::cross(p1,p2); // первое векторное произведение
c1 = glm::cross(axb,p1); // направление возвращающнй силы для узла p1
c2 = glm::cross(p2,axb); // направление возвращающнй силы для узла p2
// эквивалент действия пружины кручения на первый узел соотв. p1
a = m/(2*glm::length(p1)) * glm::normalize(c1);
// эквивалент действия пружины кручения на второй узел соотв. p2
b = m/(2*glm::length(p2)) * glm::normalize(c2);
if(!(isnan(a.x) || isnan(b.x))) {
leftNode->applyForce(a);
rightNode->applyForce(b);
centerNode->applyForce(-a - b);
}
}
}
void RigidBody::update(glm::vec3 gravity, float timeStep)
{
if (dynamic)
{
applyForce(gravity.xy*mass*timeStep);
//velocity += gravity.xy*timeStep;
position += velocity*timeStep;
if (glm::length(velocity) > 0)
{
//glm::vec2 dragVelocity = velocity;
applyForce(-velocity*linearDrag);
//velocity *= linearDrag;
}
if (glm::length(velocity) < 0.01f)
{
velocity = glm::vec2(0);
}
}
}
void PhysicsBody::resetForces()
{
cpBodyResetForces(_info->getBody());
// if _gravityEnabled is false, add a reverse of gravity force to body
if (_world != nullptr && _dynamic && !_gravityEnabled && _mass != PHYSICS_INFINITY)
{
applyForce(-_world->getGravity() * _mass);
}
}
void Mover::goBack( ){
ofVec2f forceBack = initialLoc - location;
forceBack.normalize();
forceBack.x *= 4;
forceBack.y*= 10;
// forceBack /= 4;
applyForce(forceBack);
}
/**
* @brief Body::applyForce
* @param force
* @param position
*/
void Body::applyForce(const vec2 & force, const vec2 & point)
{
if (!fixedPosition())
{
float torque = cross(point - m_vPosition, force);
applyTorque(torque);
applyForce(force.x, force.y);
m_flags &= ~SLEEPING;
}
}
void PhysicsBody::resetForces()
{
cpBodyResetForces(_info->getBody());
// if _gravityEnable is false, add a reverse of gravity force to body
if (_world != nullptr && !_gravityEnable)
{
applyForce(-_world->getGravity() * _mass);
}
}
//--------------------------------------------------------------
void vehicle::applyBehaviours(vector<vehicle> vehicles, ofVec2f* gradient){
ofPoint mouse(ofGetMouseX(), ofGetMouseY());
ofPoint separateForce = separate(vehicles);
ofPoint seekForce = seek(mouse);
ofPoint border = borders();
ofPoint slope = slopes(gradient);
separateForce*=2;
seekForce *= 1;
border *=3;
slope *= 2;
applyForce(slope);
applyForce(border);
applyForce(separateForce);
applyForce(seekForce);
}
