void UniversalJoint::act(bool initial)
{
if(!initial)
{
if(axis1->motor)
{
if(axis1->motor->getMotorType() == VELOCITY)
{
dReal desiredVelocity(dReal(axis1->motor->getDesiredVelocity()));
dJointSetUniversalParam(physicalJoint, dParamFMax, dReal(axis1->motor->getMaxForce()));
dJointSetUniversalParam(physicalJoint, dParamVel, desiredVelocity);
}
else if(axis1->motor->getMotorType() == ANGULAR)
{
double controllerOutput = axis1->motor->getControllerOutput(dJointGetUniversalAngle1(physicalJoint));
dJointSetUniversalParam(physicalJoint, dParamFMax, dReal(axis1->motor->getMaxForce()));
dJointSetUniversalParam(physicalJoint, dParamVel, dReal(controllerOutput));
}
}
if(axis2->motor)
{
if(axis2->motor->getMotorType() == VELOCITY)
{
dReal desiredVelocity(dReal(axis2->motor->getDesiredVelocity()));
dJointSetUniversalParam(physicalJoint, dParamFMax2, dReal(axis2->motor->getMaxForce()));
dJointSetUniversalParam(physicalJoint, dParamVel2, desiredVelocity);
}
else if(axis2->motor->getMotorType() == ANGULAR)
{
double controllerOutput = axis2->motor->getControllerOutput(dJointGetUniversalAngle2(physicalJoint));
dJointSetUniversalParam(physicalJoint, dParamFMax2, dReal(axis2->motor->getMaxForce()));
dJointSetUniversalParam(physicalJoint, dParamVel2, dReal(controllerOutput));
}
}
}
}
Eigen::Vector2f QS::Walk::evaluate(const Actor *theActor)
{
Eigen::Vector2f desiredVelocity(mySpeed_ms, 0.0);
Eigen::Vector2f velocity = theActor->getVelocity();
if (velocity.norm() >= mySpeed_ms)
{
desiredVelocity << 0.0, 0.0;
}
Eigen::Vector2f steeringForce = desiredVelocity *
theActor->getMass();
Eigen::Rotation2Df rotation(theActor->getOrientation());
steeringForce = rotation * steeringForce;
return steeringForce;
}
bool Player::update(float deltaTime) {
b2Vec2 velocity = _body->GetLinearVelocity();
b2Vec2 force(0.0f, 0.0f), acceleration(0.0f, 0.0f), desiredVelocity(0.0f, 0.0f);
float maxSpeed = b2Max(_maxVelocity.x, _maxVelocity.y);
//Determine animations
if (_direction.x < 0) {
_animationManager->play("BANK_LEFT");
} else if (_direction.x > 0) {
_animationManager->play("BANK_RIGHT");
} else {
if (_animationManager->getCurrentAnimationName() != "IDLE") {
Ess2D::Animation* currentAnimation = _animationManager->getCurrent();
if (!currentAnimation->isReversed()) {
currentAnimation->setReverse(true);
} else if (currentAnimation->getCurrentFrameNumber() == 0) {
_animationManager->play("IDLE");
}
}
}
//Handle Physics
if(_direction.x != 0) {
acceleration.x = (_direction.x * _maxVelocity.x - velocity.x);
} else if(velocity.x != 0) {
acceleration.x = (_defaultVelocity.x - velocity.x);
}
if(_direction.y != 0) {
if(_direction.y < 0) {
acceleration.y = _direction.y * _maxVelocity.y - velocity.y;
} else {
acceleration.y = (b2Min(_direction.y * (_maxVelocity.y + _defaultVelocity.y), velocity.y + 8.5f) - velocity.y);
}
} else {
acceleration.y = (_defaultVelocity.y - velocity.y);
}
force = _body->GetMass() * acceleration;
_body->ApplyLinearImpulse(force, _body->GetWorldCenter(), true);
float currentSpeed = velocity.Length();
if(currentSpeed > maxSpeed && _direction.y != 0) {
_body->SetLinearVelocity((maxSpeed / currentSpeed) * velocity);
}
/* BIND PLAYER WITHIN THE VIEWPORT */
//calculate next step position
glm::vec2 nextPosition = glm::vec2(
_body->GetPosition().x + _body->GetLinearVelocity().x * deltaTime + (acceleration.x * deltaTime * deltaTime) / 2,
_body->GetPosition().y + _body->GetLinearVelocity().y * deltaTime + (acceleration.y * deltaTime * deltaTime) / 2
);
glm::vec2 viewportSize = _game->getGameplayScreen()->getMainCamera()->getWorldViewportSize();
glm::vec2 cameraPosition = _game->getGameplayScreen()->getMainCamera()->getPosition() / _game->getGameplayScreen()->getMainCamera()->getZoom();
b2Vec2 correctedPosition = _body->GetPosition();
b2Vec2 correctionAcceleration = b2Vec2(0.0f, 0.0f);
b2Vec2 currentVelocity = _body->GetLinearVelocity();
bool doCorrectPosition = false;
if(nextPosition.x - _width / 2 < cameraPosition.x - viewportSize.x / 2) {
correctedPosition.x = cameraPosition.x - viewportSize.x / 2 + _width / 2;
correctionAcceleration.x = 0.0f - currentVelocity.x;
doCorrectPosition = true;
}
if(nextPosition.x + _width / 2 > cameraPosition.x + viewportSize.x / 2) {
correctedPosition.x = cameraPosition.x + viewportSize.x / 2 - _width / 2;
correctionAcceleration.x = 0.0f - currentVelocity.x;
doCorrectPosition = true;
}
if(nextPosition.y - _height / 2 < cameraPosition.y - viewportSize.y / 2 && _direction.y != 1) {
correctedPosition.y = cameraPosition.y - viewportSize.y / 2 + _height / 2;
correctionAcceleration.y = 0.0f - currentVelocity.y;
doCorrectPosition = true;
}
if(nextPosition.y + _height / 2 > cameraPosition.y + viewportSize.y / 2 && _direction.y != -1) {
correctedPosition.y = cameraPosition.y + viewportSize.y / 2 - _height / 2;
correctionAcceleration.y = _defaultVelocity.y * 0.99f - currentVelocity.y;
doCorrectPosition = true;
}
//if we have corrections to do, we must make sure to stop the body's velocity in the corrected direction as well.
//still can be a bit weird... could be interpolated camera position or something...
if(doCorrectPosition) {
b2Vec2 force = _body->GetMass() * correctionAcceleration;
//the impulse is applied in order to stop the body from moving further in that direction.
_body->ApplyLinearImpulse(force, _body->GetWorldCenter(), true);
_body->SetTransform(correctedPosition, _body->GetAngle());
}
//Update Projectile Spawners
_projectileSpawnerLeftPosition = glm::vec2(-_width / 2 + 0.5f, 0.1f);
_projectileSpawnerRightPosition = _projectileSpawnerLeftPosition + glm::vec2(_width - 1.0f, 0.0f);
correctProjectileSpawnersPosition(_animationManager->getCurrent()->getCurrentFrame());
int projectilesSpawnedLeft = _projectileSpawnerLeft.update(deltaTime, _isFiring, Utils::toVec2(_body->GetPosition()) + _projectileSpawnerLeftPosition + glm::vec2(0.0f, 1.0f), glm::vec2(0.0f, 1.0f), _body->GetAngle());
int projectilesSpawnedRight = _projectileSpawnerRight.update(deltaTime, _isFiring, Utils::toVec2(_body->GetPosition()) + _projectileSpawnerRightPosition + glm::vec2(0.0f, 1.0f), glm::vec2(0.0f, 1.0f), _body->GetAngle());
_game->getGameplayScreen()->addShotsFired(projectilesSpawnedLeft + projectilesSpawnedRight);
if(projectilesSpawnedLeft > 0) {
_muzzleLeftAnimationManager->play("MUZZLE");
_muzzleLeftAnimationManager->getCurrent()->reset();
_muzzleRightAnimationManager->play("MUZZLE");
_muzzleRightAnimationManager->getCurrent()->reset();
}
//Update Animations
_animationManager->update(deltaTime);
_thrusterAnimationManager->update(deltaTime);
_muzzleLeftAnimationManager->update(deltaTime);
_muzzleRightAnimationManager->update(deltaTime);
return true;
}
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);
}
