void GravityComponent::update(const float timeDelta, GameObject* pParentObject)
{
Vector3 impulseVector = pParentObject->getRuntimeData()->getVector("velocity");
impulseVector += getGravity() * timeDelta;
pParentObject->getRuntimeData()->insertVector(impulseVector, "velocity");
pParentObject->getRuntimeData()->insertVector(getGravity(), "gravity");
}
void CollisionDestroyPositioner::affect(float ms)
{
Entity *entity = getEntity();
CollisionDetector *collisionDetector =
entity->getLevel()->getCollisionDetector();
/* Convert vectors to ellipse space. */
Vector2D position = entity->getPosition();
Vector2D velocity = entity->getVelocity() * ms;
/* Used later. */
float time;
Vector2D collision;
if(collisionDetector->getClosestCollision(
entity->getRadius(), position, velocity,
&time, &collision)) {
entity->setGarbage();
entity->setPosition(collision);
} else {
entity->setPosition(position + velocity);
}
velocity = entity->getVelocity() + getGravity() * ms;
entity->setVelocity(velocity);
}
void GSystem::calcDynamicsWithZeroGravityAndVelocity()
{
Vec3 g = getGravity();
setGravity(Vec3(0,0,0));
double ddq[6];
for (std::list<GBody *>::iterator iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); iter_pbody++) {
(*iter_pbody)->pBaseJoint->get_ddq(ddq);
matSet_multAB((*iter_pbody)->Sddq.GetArray(), (*iter_pbody)->S.GetPtr(), ddq, 6, (*iter_pbody)->bjDOF, (*iter_pbody)->bjDOF, 1);
}
for (std::list<GBody *>::reverse_iterator riter_pbody = pBodies.rbegin(); riter_pbody != pBodies.rend(); riter_pbody++) {
(*riter_pbody)->update_aB_zeroV_zeroeta();
(*riter_pbody)->update_beta_zeroeta();
}
for (std::list<GBody *>::iterator iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); iter_pbody++) {
if ( (*iter_pbody)->pBaseJoint->isPrescribed() ) {
(*iter_pbody)->update_dV(false);
(*iter_pbody)->update_F_fs();
(*iter_pbody)->update_tau();
} else {
(*iter_pbody)->update_ddq();
(*iter_pbody)->update_dV(true);
//(*iter_pbody)->update_F_fs(); // we do not need this.
}
}
setGravity(g);
}
void PlayerPhysicsObject::handleInput(float delta)
{
std::vector<int> playerAttributes = itrPhysics_3.ownerAt(attributeIndex_)->getAttributes(ATTRIBUTE_PLAYER);
if(playerAttributes.size() > 1)
{
ERROR_MESSAGEBOX("More than one controller for one player. Not tested.")
}
for(unsigned int i=0; i<playerAttributes.size(); i++)
{
AttributePtr<Attribute_Player> ptr_player = ptr_player = itrPlayer.at(playerAttributes.at(i));
AttributePtr<Attribute_Input> ptr_input = ptr_player->ptr_input;
AttributePtr<Attribute_Health> health = ptr_player->ptr_health;
if(health->health <= 0)
{
continue;
}
//--------------------------------------------------------------------------------------
//Look and move
//--------------------------------------------------------------------------------------
yaw_ += ptr_input->rotation.x;
btVector3 move = ptr_player->currentSpeed*btVector3(ptr_input->position.x, 0, ptr_input->position.y);
//lower player speed when recently damaged
if(ptr_player->timeSinceLastDamageTaken < 1.0f)
{
move *= 0.75f;
}
//Move player
move = move.rotate(btVector3(0,1,0),yaw_);
move = btVector3(move.x(), getLinearVelocity().y(), move.z());
setLinearVelocity(move);
//Rotate player
btTransform world;
world = getWorldTransform();
world.setRotation(btQuaternion(yaw_,0,0));
setWorldTransform(world);
//Jetpack
if(ptr_player->jetpack)
{
float jetpackPower = -getGravity().y()*1.5f;
world = getWorldTransform();
btVector3 velocity = getLinearVelocity();
if(world.getOrigin().y() < 18.0f)
{
setLinearVelocity(btVector3(move.x(), velocity.y()+jetpackPower*delta, move.z()));
}
}
else if(ptr_input->jump && ptr_player->hovering) //Jump
{
float jumpPower = 600.0f;
applyCentralImpulse(btVector3(0.0f, jumpPower, 0.0f));
//applyCentralForce(btVector3(0.0f, jumpPower, 0.0f));
}
}
}
gReal GSystem::getGravitationalPotentialEnergy()
{
gReal e = 0;
for (std::list<GBody *>::iterator iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); iter_pbody++) {
e += Inner((*iter_pbody)->getPositionCOMGlobal(), -getGravity()) * (*iter_pbody)->getMass(); // e += <c,-mg>
}
return e;
}
bool GSystem::calcProductOfInvMassAndMatrix(RMatrix &invM_A, const RMatrix &A)
{
if ( (size_t)A.RowSize() != pCoordinates.size() ) return false;
invM_A.SetZero(A.RowSize(), A.ColSize());
int i;
std::list<GJoint *>::iterator iter_pjoint;
std::vector<bool> isprescribed(pJoints.size());
// save current info
for (i=0, iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); i++, iter_pjoint++) {
isprescribed[i] = (*iter_pjoint)->isPrescribed();
}
Vec3 g = getGravity();
// set all joint unprescribed and set zero gravity
setAllJointsPrescribed(false);
setGravity(Vec3(0,0,0));
update_joint_local_info_short();
for (std::list<GBody *>::iterator iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); iter_pbody++) {
(*iter_pbody)->update_base_joint_info();
(*iter_pbody)->update_T();
(*iter_pbody)->set_eta_zero();
}
for (std::list<GBody *>::reverse_iterator riter_pbody = pBodies.rbegin(); riter_pbody != pBodies.rend(); riter_pbody++) {
(*riter_pbody)->update_aI();
(*riter_pbody)->update_Psi();
(*riter_pbody)->update_Pi();
}
for (i=0; i<A.ColSize(); i++) {
set_ddq(Zeros(pCoordinates.size(),1)); // this isn't necessary for real tree structure systems, but works for the cut joints in closed-loop
set_tau(&(A[i*A.RowSize()]));
for (std::list<GBody *>::reverse_iterator riter_pbody = pBodies.rbegin(); riter_pbody != pBodies.rend(); riter_pbody++) {
(*riter_pbody)->update_aB_zeroV_zeroeta();
(*riter_pbody)->update_beta_zeroeta();
}
for (std::list<GBody *>::iterator iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); iter_pbody++) {
(*iter_pbody)->update_ddq();
(*iter_pbody)->update_dV(true);
}
get_ddq(&(invM_A[i*invM_A.RowSize()]));
}
// restore
for (i=0, iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); i++, iter_pjoint++) {
(*iter_pjoint)->setPrescribed(isprescribed[i]);
}
setGravity(g);
return true;
}
void ContinuedGameMenuWindow::on_stage4Button_clicked()
{
//switch to debate window
emit setOpponent(4);
emit switchToWindow(7);
emit getQuestions();
emit stageSelected(4);
emit updatePhysicsWorld(getGravity(), getBounce(), getOppBounce());
}
void actionUpdate() {
jump_power_ += jump_velocity_;
jump_power_ <= 0.0f
? jump_power_ = 0.0f
: jump_velocity_ += getGravity();
if (!isJumping()) jump_velocity_ = 0.0f;
}
//-----------------------------------------------------------------------------
//
// VActorPhysicsController::applyGravity( pElapsedTime );
//
// Apply gravity for the elapsed period.
//
//-----------------------------------------------------------------------------
void VActorPhysicsController::applyGravity( const F32 &pElapsedTime )
{
// Get Velocity.
VectorF velocity = getVelocity();
// Add Tick Gravity.
velocity += getGravity() * pElapsedTime;
// Apply.
setVelocity( velocity );
}
void PhysicsObject::writeNonSynchronizedPhysicsObjectDataToPhysicsAttribute()
{
AttributePtr<Attribute_Physics> ptr_physics = itrPhysics_.at(attributeIndex_);
ptr_physics->angularVelocity = convert(&getAngularVelocity());
ptr_physics->collisionFilterGroup = getCollisionFilterGroup();
ptr_physics->collisionResponse = (getCollisionFlags() & btCollisionObject::CF_NO_CONTACT_RESPONSE) == 0;
ptr_physics->gravity = convert(&getGravity());
ptr_physics->linearVelocity = convert(&getLinearVelocity());
//ptr_physics->collisionFilterMask =
//ptr_physics->mass = physicsObject->getInvMass(); //only mass inverse is stored in physics object
//ptr_physics->meshID = //not stored in physics object
}
uint8_t Accelerometer::getYawPitchRoll(float *data)
{
uint8_t fifoBuffer[64];
Quaternion q;
VectorFloat gravity;
uint16_t packetSize = getFIFOPacketSize();
getFIFOBytes(fifoBuffer, packetSize);
getQuaternion(&q, fifoBuffer);
getGravity(&gravity, &q);
getYawPitchRoll(data, &q, &gravity);
data[0] = (data[0] * 180/M_PI) - zeroValues[0];
data[1] = (data[1] * 180/M_PI) - zeroValues[1];
data[2] = (data[2] * 180/M_PI) - zeroValues[2];
return 0;
}
void CPhysics::UpdatePeeing(std::vector<CParticle*>myParticleList, std::vector<unsigned char> &heightMap, const Vector3& terrainSize, Camera3& camera, double& dt)
{
//this should trigger when the right mouse click is triggered
for(std::vector<CParticle* >::iterator it = myParticleList.begin(); it != myParticleList.end(); ++it)
{
CParticle* go = (CParticle *)*it;
if(go->type == CParticle::PARTICLE_PEE)
{
go->vel += getGravity() * (float)dt;
go->pos += go->vel * (float)dt;
}
if(go->pos.y <= GetHeightMapY(go->pos.x, go->pos.z, heightMap, terrainSize))
{
go->active = false;
}
}
}
void PhysicsSystem::HandleEvents(const frame_tp& timepoint) {
// Remove access to old updated transforms
TransformMap::GetAsyncUpdatedTransforms().Unpublish(timepoint);
// Remove access to forces
async_forces.Unpublish(timepoint);
// Remove access to torques
async_torques.Unpublish(timepoint);
// publish the forces of the current frame immediately without making a copy of the list
async_forces.Publish(std::make_shared<const std::map<id_t,btVector3>>(this->forces.Poll()));
// publish the torques of the current frame
async_torques.Publish(std::make_shared<const std::map<id_t,btVector3>>(this->torques.Poll()));
static frame_tp last_tp;
this->delta = timepoint - last_tp;
last_tp = timepoint;
// Set the rigid bodies linear velocity. Must be done each frame otherwise,
// other forces will stop the linear velocity.
// We use the published list
for (auto& force : *async_forces.GetFuture(timepoint).get()) {
auto body = this->bodies[force.first]->GetRigidBody();
body->setLinearVelocity(force.second + body->getGravity());
}
// Set the rigid bodies angular velocity. Must be done each frame otherwise,
// other forces will stop the angular velocity.
for (auto& torque : *async_torques.GetFuture(timepoint).get()) {
auto body = this->bodies[torque.first]->GetRigidBody();
body->setAngularVelocity(torque.second);
}
if (this->dynamicsWorld) {
dynamicsWorld->stepSimulation(delta.count() * 1.0E-9, 10);
}
// Set out transform updates.
for (auto& shape : this->bodies) {
shape.second->UpdateTransform();
}
// Publish the new updated transforms map
auto ntm = std::make_shared<std::map<id_t,const Transform*>>(TransformMap::GetUpdatedTransforms().Poll());
TransformMap::GetAsyncUpdatedTransforms().Publish(std::move(ntm));
}
void EntityItem::update(const quint64& updateTime) {
bool wantDebug = false;
if (_lastUpdated == 0) {
_lastUpdated = updateTime;
}
float timeElapsed = (float)(updateTime - _lastUpdated) / (float)(USECS_PER_SECOND);
if (wantDebug) {
qDebug() << "********** EntityItem::update()";
qDebug() << " entity ID=" << getEntityItemID();
qDebug() << " updateTime=" << updateTime;
qDebug() << " _lastUpdated=" << _lastUpdated;
qDebug() << " timeElapsed=" << timeElapsed;
qDebug() << " hasVelocity=" << hasVelocity();
qDebug() << " hasGravity=" << hasGravity();
qDebug() << " isRestingOnSurface=" << isRestingOnSurface();
qDebug() << " hasAngularVelocity=" << hasAngularVelocity();
qDebug() << " getAngularVelocity=" << getAngularVelocity();
qDebug() << " isMortal=" << isMortal();
qDebug() << " getAge()=" << getAge();
qDebug() << " getLifetime()=" << getLifetime();
if (hasVelocity() || (hasGravity() && !isRestingOnSurface())) {
qDebug() << " MOVING...=";
qDebug() << " hasVelocity=" << hasVelocity();
qDebug() << " hasGravity=" << hasGravity();
qDebug() << " isRestingOnSurface=" << isRestingOnSurface();
qDebug() << " hasAngularVelocity=" << hasAngularVelocity();
qDebug() << " getAngularVelocity=" << getAngularVelocity();
}
if (hasAngularVelocity()) {
qDebug() << " CHANGING...=";
qDebug() << " hasAngularVelocity=" << hasAngularVelocity();
qDebug() << " getAngularVelocity=" << getAngularVelocity();
}
if (isMortal()) {
qDebug() << " MORTAL...=";
qDebug() << " isMortal=" << isMortal();
qDebug() << " getAge()=" << getAge();
qDebug() << " getLifetime()=" << getLifetime();
}
}
_lastUpdated = updateTime;
if (wantDebug) {
qDebug() << " ********** EntityItem::update() .... SETTING _lastUpdated=" << _lastUpdated;
}
if (hasAngularVelocity()) {
glm::quat rotation = getRotation();
glm::vec3 angularVelocity = glm::radians(getAngularVelocity());
float angularSpeed = glm::length(angularVelocity);
if (angularSpeed < EPSILON_VELOCITY_LENGTH) {
setAngularVelocity(NO_ANGULAR_VELOCITY);
} else {
float angle = timeElapsed * angularSpeed;
glm::quat dQ = glm::angleAxis(angle, glm::normalize(angularVelocity));
rotation = dQ * rotation;
setRotation(rotation);
// handle damping for angular velocity
if (getAngularDamping() > 0.0f) {
glm::vec3 dampingResistance = getAngularVelocity() * getAngularDamping();
glm::vec3 newAngularVelocity = getAngularVelocity() - (dampingResistance * timeElapsed);
setAngularVelocity(newAngularVelocity);
if (wantDebug) {
qDebug() << " getDamping():" << getDamping();
qDebug() << " dampingResistance:" << dampingResistance;
qDebug() << " newAngularVelocity:" << newAngularVelocity;
}
}
}
}
if (hasVelocity() || hasGravity()) {
glm::vec3 position = getPosition();
glm::vec3 velocity = getVelocity();
glm::vec3 newPosition = position + (velocity * timeElapsed);
if (wantDebug) {
qDebug() << " EntityItem::update()....";
qDebug() << " timeElapsed:" << timeElapsed;
qDebug() << " old AACube:" << getMaximumAACube();
qDebug() << " old position:" << position;
qDebug() << " old velocity:" << velocity;
qDebug() << " old getAABox:" << getAABox();
qDebug() << " getDistanceToBottomOfEntity():" << getDistanceToBottomOfEntity() * (float)TREE_SCALE << " in meters";
qDebug() << " newPosition:" << newPosition;
qDebug() << " glm::distance(newPosition, position):" << glm::distance(newPosition, position);
}
position = newPosition;
// handle bounces off the ground... We bounce at the distance to the bottom of our entity
if (position.y <= getDistanceToBottomOfEntity()) {
velocity = velocity * glm::vec3(1,-1,1);
// if we've slowed considerably, then just stop moving
if (glm::length(velocity) <= EPSILON_VELOCITY_LENGTH) {
velocity = NO_VELOCITY;
}
position.y = getDistanceToBottomOfEntity();
}
// handle gravity....
if (hasGravity() && !isRestingOnSurface()) {
velocity += getGravity() * timeElapsed;
}
// handle resting on surface case, this is definitely a bit of a hack, and it only works on the
// "ground" plane of the domain, but for now it
if (hasGravity() && isRestingOnSurface()) {
velocity.y = 0.0f;
position.y = getDistanceToBottomOfEntity();
}
// handle damping for velocity
glm::vec3 dampingResistance = velocity * getDamping();
if (wantDebug) {
qDebug() << " getDamping():" << getDamping();
qDebug() << " dampingResistance:" << dampingResistance;
qDebug() << " dampingResistance * timeElapsed:" << dampingResistance * timeElapsed;
}
velocity -= dampingResistance * timeElapsed;
if (wantDebug) {
qDebug() << " velocity AFTER dampingResistance:" << velocity;
qDebug() << " glm::length(velocity):" << glm::length(velocity);
qDebug() << " EPSILON_VELOCITY_LENGTH:" << EPSILON_VELOCITY_LENGTH;
}
// round velocity to zero if it's close enough...
if (glm::length(velocity) <= EPSILON_VELOCITY_LENGTH) {
velocity = NO_VELOCITY;
}
setPosition(position); // this will automatically recalculate our collision shape
setVelocity(velocity);
if (wantDebug) {
qDebug() << " new position:" << position;
qDebug() << " new velocity:" << velocity;
qDebug() << " new AACube:" << getMaximumAACube();
qDebug() << " old getAABox:" << getAABox();
}
}
}
void Simulation::configure(const config::Configuration& config)
{
// Resize world
{
auto size = config.get<SizeVector>("world-size");
if (size.getWidth() == Zero || size.getHeight() == Zero)
throw config::Exception("Width or height is zero!");
setWorldSize(size);
}
// Time step
setTimeStep(config.get<units::Time>("dt"));
if (config.has("length-coefficient"))
{
m_converter.setLengthCoefficient(config.get<RealType>("length-coefficient"));
}
// Set gravity
setGravity(config.get("gravity", getGravity()));
// Number of iterations
setIterations(config.get("iterations", getIterations()));
// Background color
setBackgroundColor(config.get("background", getBackgroundColor()));
#if CONFIG_RENDER_TEXT_ENABLE
setFontColor(config.get("text-color", getBackgroundColor().inverted()));
#endif
#if CONFIG_RENDER_TEXT_ENABLE
setFontSize(config.get("text-size", getFontSize()));
#endif
#if CONFIG_RENDER_TEXT_ENABLE
setSimulationTimeRender(config.get("show-simulation-time", isSimulationTimeRender()));
#endif
#ifdef CECE_ENABLE_RENDER
setVisualized(config.get("visualized", isVisualized()));
#endif
// Parse plugins
for (auto&& pluginConfig : config.getConfigurations("plugin"))
{
// Returns valid pointer or throws an exception
requirePlugin(pluginConfig.get("name"))->configure(*this, pluginConfig);
}
// Parse parameters
for (auto&& parameterConfig : config.getConfigurations("parameter"))
{
setParameter(parameterConfig.get("name"), units::parse(parameterConfig.get("value")));
}
// Register user types
for (auto&& typeConfig : config.getConfigurations("type"))
{
addObjectType({
typeConfig.get("name"),
typeConfig.get("base"),
typeConfig.toMemory()
});
}
// Parse init
for (auto&& initConfig : config.getConfigurations("init"))
{
const String typeName = initConfig.has("language")
? initConfig.get("language")
: initConfig.get("type");
auto initializer = getPluginContext().createInitializer(typeName);
if (initializer)
{
// Configure initializer
initializer->loadConfig(*this, initConfig);
// Register initializer
addInitializer(std::move(initializer));
}
}
// Parse modules
for (auto&& moduleConfig : config.getConfigurations("module"))
{
// Get name
auto name = moduleConfig.get("name");
if (hasModule(name))
continue;
const String typeName = moduleConfig.has("language")
? moduleConfig.get("language")
: moduleConfig.has("type")
? moduleConfig.get("type")
: name
;
auto module = getPluginContext().createModule(typeName, *this);
if (module)
{
module->loadConfig(*this, moduleConfig);
addModule(std::move(name), std::move(module));
}
}
// Parse programs
for (auto&& programConfig : config.getConfigurations("program"))
{
const String typeName = programConfig.has("language")
? programConfig.get("language")
: programConfig.get("type");
auto program = getPluginContext().createProgram(typeName);
if (program)
{
// Configure program
program->loadConfig(*this, programConfig);
// Register program
addProgram(programConfig.get("name"), std::move(program));
}
}
// Parse objects
for (auto&& objectConfig : config.getConfigurations("object"))
{
// Create object
auto object = buildObject(
objectConfig.get("class"),
objectConfig.get("type", object::Object::Type::Dynamic)
);
if (object)
object->configure(objectConfig, *this);
}
if (config.has("data-out-objects-filename"))
{
m_dataOutObjects = makeUnique<OutFileStream>(config.get("data-out-objects-filename"));
*m_dataOutObjects << "iteration;totalTime;id;typeName;posX;posY;velX;velY\n";
}
}
//--------------------------------------------------------------------------
float Physics::getVerticalPosition(int initialVelocity, int trajectoryAngle, float time)
{
// !NOTE! Make sure the trajectoryAngle is in radians.
return (initialVelocity * Math::getSin(trajectoryAngle)) * time - (getGravity() / 2.0f) * (time * time);
}
void PhysicsWorld::step(float deltaTime)
{
//TODO:
/*
how to clear out friction? currently, no friction
*/
Player* player = GlobalEngine::sharedGlobalEngine()->getPlayer();
CCArray* planets = GlobalEngine::sharedGlobalEngine()->getLevelMapLayer()->getPlanets();
//add gravity
b2Vec2 force;
force.SetZero();
b2Body* playerBody = player->getB2Body();
if (player->isGrounded() && player->getCurrentGround()->getType() == GameObjectTypePlanet)
{
//only apply gravity of current planet
force = getGravity(player, (Planet*)player->getCurrentGround());
}
else
{
for (int i = 0; i < planets->count(); i++)
{
force += getGravity(player, (Planet*)planets->objectAtIndex(i));
}
}
//rotate to gravity
float angle = atan2(force.y, force.x) + b2_pi / 2.0f;
playerBody->SetTransform(playerBody->GetPosition(), angle);
playerBody->ApplyForceToCenter(force);
//apply movement by force
/*b2Vec2 localForce;
switch (_player->getMoveState()) {
case PlayerMoveStateLeft:
localForce.Set(-PLAYER_MOVE_FORCE, 0);
break;
case PlayerMoveStateRight:
localForce.Set(PLAYER_MOVE_FORCE, 0);
break;
default:
localForce.SetZero();
break;
}
b2Rot rotation(angle);
b2Vec2 globalForce = b2Mul(rotation, localForce);
playerBody->ApplyForceToCenter(globalForce);
//b2Vec2 impulse = globalSpeed - playerBody->GetLinearVelocity();
//impulse *= playerBody->GetMass();
//playerBody->ApplyLinearImpulse(impulse, playerBody->GetWorldCenter());
*/
//apply movement by impulse
if (player->isGrounded() && player->getJumpState() != PlayerJumpStateLeaving)
{
b2Vec2 localSpeed;
switch (player->getMoveState()) {
case PlayerMoveStateLeft:
localSpeed.Set(-PLAYER_MOVE_SPEED, 0);
break;
case PlayerMoveStateRight:
localSpeed.Set(PLAYER_MOVE_SPEED, 0);
break;
default:
localSpeed.SetZero();
break;
}
b2Rot rotation(angle);
b2Vec2 globalSpeed = b2Mul(rotation, localSpeed);
b2Vec2 impulse = globalSpeed - playerBody->GetLinearVelocity();
impulse *= playerBody->GetMass();
playerBody->ApplyLinearImpulse(impulse, playerBody->GetWorldCenter());
}
//apply jumping
if (player->getJumpState() == PlayerJumpStatePending)
{
player->setJumpState(PlayerJumpStateLeaving);
if (player->isGrounded())
{
b2Vec2 localJumpSpeed;
localJumpSpeed.Set(PLAYER_JUMP_SPEED, 0);
b2Rot jumpRotation(angle + b2_pi / 2.0f);
b2Vec2 globalJumpSpeed = b2Mul(jumpRotation, localJumpSpeed);
b2Vec2 jumpImpulse = globalJumpSpeed;
jumpImpulse *= playerBody->GetMass();
playerBody->ApplyLinearImpulse(jumpImpulse, playerBody->GetWorldCenter());
}
}
//check damping
if (player->isGrounded())
{
playerBody->SetLinearDamping(0);
}
else
{
playerBody->SetLinearDamping(PLAYER_DAMPING);
}
_world->Step(deltaTime, VELOCITY_ITERATIONS, POSITION_ITERATIONS);
}
Vec2 PhysicsForceField::getGravity(Vec2 p)
{
cpVect res = getGravity(cpVect{p.x, p.y});
return Vec2(res.x, res.y);
}
void ADXL335::update()
{
_xg = getGravity(analogRead(_pin_x));
_yg = getGravity(analogRead(_pin_y));
_zg = getGravity(analogRead(_pin_z));
}
//==============================================================================
TEST(Issue1184, Accuracy)
{
struct ShapeInfo
{
dart::dynamics::ShapePtr shape;
double offset;
};
std::function<ShapeInfo()> makePlaneGround =
[]()
{
return ShapeInfo{
std::make_shared<dart::dynamics::PlaneShape>(
Eigen::Vector3d::UnitZ(), 0.0),
0.0};
};
std::function<ShapeInfo()> makeBoxGround =
[]()
{
const double thickness = 0.1;
return ShapeInfo{
std::make_shared<dart::dynamics::BoxShape>(
Eigen::Vector3d(100.0, 100.0, thickness)),
-thickness/2.0};
};
std::function<dart::dynamics::ShapePtr(double)> makeBoxObject =
[](const double s) -> dart::dynamics::ShapePtr
{
return std::make_shared<dart::dynamics::BoxShape>(
Eigen::Vector3d::Constant(2*s));
};
std::function<dart::dynamics::ShapePtr(double)> makeSphereObject =
[](const double s) -> dart::dynamics::ShapePtr
{
return std::make_shared<dart::dynamics::SphereShape>(s);
};
#ifndef NDEBUG
const auto groundInfoFunctions = {makePlaneGround};
const auto objectShapeFunctions = {makeSphereObject};
const auto halfsizes = {10.0};
const auto fallingModes = {true};
const double dropHeight = 0.1;
const double tolerance = 1e-3;
#else
const auto groundInfoFunctions = {makePlaneGround, makeBoxGround};
const auto objectShapeFunctions = {makeBoxObject, makeSphereObject};
const auto halfsizes = {0.25, 1.0, 5.0, 10.0, 20.0};
const auto fallingModes = {true, false};
const double dropHeight = 1.0;
const double tolerance = 1e-3;
#endif
for(const auto& groundInfoFunction : groundInfoFunctions)
{
for(const auto& objectShapeFunction : objectShapeFunctions)
{
for(const double halfsize : halfsizes)
{
for(const bool falling : fallingModes)
{
auto world = dart::simulation::World::create("test");
world->getConstraintSolver()->setCollisionDetector(
dart::collision::BulletCollisionDetector::create());
Eigen::Isometry3d tf_object = Eigen::Isometry3d::Identity();
const double initialHeight = falling? dropHeight+halfsize : halfsize;
tf_object.translate(initialHeight*Eigen::Vector3d::UnitZ());
auto object = dart::dynamics::Skeleton::create("ball");
object->createJointAndBodyNodePair<dart::dynamics::FreeJoint>()
.first->setTransform(tf_object);
const auto objectShape = objectShapeFunction(halfsize);
object->getBodyNode(0)->createShapeNodeWith<
dart::dynamics::VisualAspect,
dart::dynamics::CollisionAspect>(objectShape);
world->addSkeleton(object);
const ShapeInfo groundInfo = groundInfoFunction();
auto ground = dart::dynamics::Skeleton::create("ground");
ground->createJointAndBodyNodePair<dart::dynamics::WeldJoint>()
.second->createShapeNodeWith<
dart::dynamics::VisualAspect,
dart::dynamics::CollisionAspect>(groundInfo.shape);
Eigen::Isometry3d tf_ground = Eigen::Isometry3d::Identity();
tf_ground.translate(groundInfo.offset*Eigen::Vector3d::UnitZ());
ground->getJoint(0)->setTransformFromParentBodyNode(tf_ground);
world->addSkeleton(ground);
// time until the object will strike
const double t_strike = falling?
sqrt(-2.0*dropHeight/world->getGravity()[2]) : 0.0;
// give the object some time to settle
const double min_time = 0.5;
const double t_limit = 30.0*t_strike + min_time;
double lowestHeight = std::numeric_limits<double>::infinity();
double time = 0.0;
while(time < t_limit)
{
world->step();
const double currentHeight =
object->getBodyNode(0)->getTransform().translation()[2];
if(currentHeight < lowestHeight)
lowestHeight = currentHeight;
time = world->getTime();
}
// The simulation should have run for at least two seconds
ASSERT_LE(min_time, time);
EXPECT_GE(halfsize+tolerance, lowestHeight)
<< "object type: " << objectShape->getType()
<< "\nground type: " << groundInfo.shape->getType()
<< "\nfalling: " << falling << "\n";
const double finalHeight =
object->getBodyNode(0)->getTransform().translation()[2];
EXPECT_NEAR(halfsize, finalHeight, tolerance)
<< "object type: " << objectShape->getType()
<< "\nground type: " << groundInfo.shape->getType()
<< "\nfalling: " << falling << "\n";
}
}
}
}
}
void World::update(float dt)
{
Player* player = NULL;
// Update all the objects.
for(int i = 0; i < mObjectList.size(); i++)
{
Object3D* object = mObjectList[i];
// Remove from the list if dead.
if(!object->getAlive()) {
// Inform the current wave about the dead enemy. [TODO] Check if we are in PlayState.
if(object->getType() == ENEMY)
PlayState::Instance()->getCurrentWave()->enemyKilled();
removeObject(object);
continue;
}
// Get the distance above the ground.
float distance = object->getPosition().y - mTerrain->getHeight(object->getPosition().x, object->getPosition().z);
// Snap to ground.
// Note the +50 to give it some margin (only for enemies).
float margin = object->getType() != PLAYER ? 50 : 0;
if(distance < object->getHeightOffset() + margin && object->getVelocity().y <= 0)
{
object->setPosition(object->getPosition() - D3DXVECTOR3(0, distance, 0) + D3DXVECTOR3(0, object->getHeightOffset(), 0));
object->setVelocity(D3DXVECTOR3(object->getVelocity().x, 0, object->getVelocity().z));
}
// Set on ground.
if(distance < object->getHeightOffset() + 0.2)
object->setOnGround(true);
else
object->setOnGround(false);
// Update the object.
object->update(dt);
// Gravity.
object->accelerate(0, getGravity(), 0);
// Friction.
if(object->getOnGround())
{
D3DXVECTOR3 norm;
D3DXVec3Normalize(&norm, &object->getVelocity());
D3DXVECTOR3 velocity = norm * mFriction * object->getFriction();//object->getVelocity()
object->accelerate(velocity.x, 0.0f, velocity.z);
}
// Move the object with it's speed.
// [NOTE] Only update the objects position here! Change the speed on other places instead!
D3DXVECTOR3 speed = object->getVelocity();
object->move(speed.x, speed.y, speed.z);
if(object->getType() == PLAYER)
player = dynamic_cast<Player*>(object);
}
// [HACK]
// Test if player is in range of a powerup.
if(player != NULL)
{
for(int i = 0; i < mObjectList.size(); i++) {
if(mObjectList[i]->getType() == ENERGY_POWERUP) {
D3DXVECTOR3 dist = mObjectList[i]->getPosition() - player->getPosition();
dist.y = 0.0f;
float d = sqrt(dist.x*dist.x + dist.z*dist.z);
if(d < 70) // PICKUP_RADIUS
(dynamic_cast<Powerup*>(mObjectList[i]))->pickup(player);
}
else
continue;
}
}
// Terrain editing.
// [NOTE] Disabled.
//editTerrain();
}
void Player::update(sf::Time dt)
{
if(mIsMovingLeft || mIsMovingRight || mIsJumping)
mMovement.x*=mFrictionStart;
else
mMovement.x*=mFrictionStop;
if(mIsJumping)
mMovement.y+=mFrictionVertical;
sf::FloatRect playerRect = getTransform().transformRect(mAnimation.getGlobalBounds());
playerRect.left+=mMovement.x*dt.asSeconds();
playerRect.top+=mMovement.y*dt.asSeconds();
// Update player's collision points
mCollPoints.up[0]=sf::Vector2f(playerRect.left+mCollMarg,playerRect.top);
mCollPoints.up[1]=sf::Vector2f(playerRect.left+playerRect.width-mCollMarg,playerRect.top);
mCollPoints.down[0]=sf::Vector2f(playerRect.left+mCollMarg,playerRect.top+playerRect.height);
mCollPoints.down[1]=sf::Vector2f(playerRect.left+playerRect.width-mCollMarg,playerRect.top+playerRect.height);
mCollPoints.left[0]=sf::Vector2f(playerRect.left,playerRect.top+mCollMarg);
mCollPoints.left[1]=sf::Vector2f(playerRect.left,playerRect.top+playerRect.height-mCollMarg);
mCollPoints.left[2]=sf::Vector2f(playerRect.left,playerRect.top+playerRect.height/2);
mCollPoints.right[0]=sf::Vector2f(playerRect.left+playerRect.width,playerRect.top+mCollMarg);
mCollPoints.right[1]=sf::Vector2f(playerRect.left+playerRect.width,playerRect.top+playerRect.height-mCollMarg);
mCollPoints.right[2]=sf::Vector2f(playerRect.left+playerRect.width,playerRect.top+playerRect.height/2);
//Collision detection
collisionDetection(playerRect);
pickupCollisionDetection(playerRect);
if(mCollisionRight)
if(mMovement.x>0)
mMovement.x*=-0.1f;
if(mCollisionLeft)
if(mMovement.x<0)
mMovement.x*=-0.1f;
if(mCollisionUp)
if(mMovement.y<0)
mMovement.y*=-0.1f;
if(mCollisionDown)
{
if(!(mIsMovingLeft || mIsMovingRight || mPlayerDead))
mAnimation.playAnimation("stay");
if(mMovement.y>0)
// mMovement.y*=-0.1f;
mMovement.y=0;
mIsJumping=false;
}
else
{
/*if(mAnimation.getCurrentAnimation()!="jump")
mAnimation.playAnimation("fall");*/
mIsJumping=true;
mMovement.y+=getGravity();
}
mAnimation.update(dt);
move(mMovement*dt.asSeconds());
if(mEnemyManager->intersection(getAABB()))
{
mPlayerDead=true;
mAnimation.playAnimation("dead");
}
if(mStar->getGlobalBounds().intersects(getAABB()))
mLevelCompleted=true;
//ss.str("");
//ss << "Left: "<<(int)mCollisionLeft<< "\nRight: "<<(int)mCollisionRight<<"\nUp: "<<(int)mCollisionUp<<"\nDown: "<<(int)mCollisionDown;// put float into string buffer
//Reset variables
mIsMovingLeft=false;
mIsMovingRight=false;
mCollisionDown=false;
mCollisionLeft=false;
mCollisionRight=false;
mCollisionUp=false;
}
OctreeElement::AppendState EntityItem::appendEntityData(OctreePacketData* packetData, EncodeBitstreamParams& params,
EntityTreeElementExtraEncodeData* entityTreeElementExtraEncodeData) const {
// ALL this fits...
// object ID [16 bytes]
// ByteCountCoded(type code) [~1 byte]
// last edited [8 bytes]
// ByteCountCoded(last_edited to last_updated delta) [~1-8 bytes]
// PropertyFlags<>( everything ) [1-2 bytes]
// ~27-35 bytes...
OctreeElement::AppendState appendState = OctreeElement::COMPLETED; // assume the best
// encode our ID as a byte count coded byte stream
QByteArray encodedID = getID().toRfc4122();
// encode our type as a byte count coded byte stream
ByteCountCoded<quint32> typeCoder = getType();
QByteArray encodedType = typeCoder;
quint64 updateDelta = getLastUpdated() <= getLastEdited() ? 0 : getLastUpdated() - getLastEdited();
ByteCountCoded<quint64> updateDeltaCoder = updateDelta;
QByteArray encodedUpdateDelta = updateDeltaCoder;
EntityPropertyFlags propertyFlags(PROP_LAST_ITEM);
EntityPropertyFlags requestedProperties = getEntityProperties(params);
EntityPropertyFlags propertiesDidntFit = requestedProperties;
// If we are being called for a subsequent pass at appendEntityData() that failed to completely encode this item,
// then our entityTreeElementExtraEncodeData should include data about which properties we need to append.
if (entityTreeElementExtraEncodeData && entityTreeElementExtraEncodeData->entities.contains(getEntityItemID())) {
requestedProperties = entityTreeElementExtraEncodeData->entities.value(getEntityItemID());
}
LevelDetails entityLevel = packetData->startLevel();
quint64 lastEdited = getLastEdited();
const bool wantDebug = false;
if (wantDebug) {
float editedAgo = getEditedAgo();
QString agoAsString = formatSecondsElapsed(editedAgo);
qDebug() << "Writing entity " << getEntityItemID() << " to buffer, lastEdited =" << lastEdited
<< " ago=" << editedAgo << "seconds - " << agoAsString;
}
bool successIDFits = false;
bool successTypeFits = false;
bool successCreatedFits = false;
bool successLastEditedFits = false;
bool successLastUpdatedFits = false;
bool successPropertyFlagsFits = false;
int propertyFlagsOffset = 0;
int oldPropertyFlagsLength = 0;
QByteArray encodedPropertyFlags;
int propertyCount = 0;
successIDFits = packetData->appendValue(encodedID);
if (successIDFits) {
successTypeFits = packetData->appendValue(encodedType);
}
if (successTypeFits) {
successCreatedFits = packetData->appendValue(_created);
}
if (successCreatedFits) {
successLastEditedFits = packetData->appendValue(lastEdited);
}
if (successLastEditedFits) {
successLastUpdatedFits = packetData->appendValue(encodedUpdateDelta);
}
if (successLastUpdatedFits) {
propertyFlagsOffset = packetData->getUncompressedByteOffset();
encodedPropertyFlags = propertyFlags;
oldPropertyFlagsLength = encodedPropertyFlags.length();
successPropertyFlagsFits = packetData->appendValue(encodedPropertyFlags);
}
bool headerFits = successIDFits && successTypeFits && successCreatedFits && successLastEditedFits
&& successLastUpdatedFits && successPropertyFlagsFits;
int startOfEntityItemData = packetData->getUncompressedByteOffset();
if (headerFits) {
bool successPropertyFits;
propertyFlags -= PROP_LAST_ITEM; // clear the last item for now, we may or may not set it as the actual item
// These items would go here once supported....
// PROP_PAGED_PROPERTY,
// PROP_CUSTOM_PROPERTIES_INCLUDED,
APPEND_ENTITY_PROPERTY(PROP_POSITION, appendPosition, getPosition());
APPEND_ENTITY_PROPERTY(PROP_DIMENSIONS, appendValue, getDimensions()); // NOTE: PROP_RADIUS obsolete
if (wantDebug) {
qDebug() << " APPEND_ENTITY_PROPERTY() PROP_DIMENSIONS:" << getDimensions();
}
APPEND_ENTITY_PROPERTY(PROP_ROTATION, appendValue, getRotation());
APPEND_ENTITY_PROPERTY(PROP_MASS, appendValue, getMass());
APPEND_ENTITY_PROPERTY(PROP_VELOCITY, appendValue, getVelocity());
APPEND_ENTITY_PROPERTY(PROP_GRAVITY, appendValue, getGravity());
APPEND_ENTITY_PROPERTY(PROP_DAMPING, appendValue, getDamping());
APPEND_ENTITY_PROPERTY(PROP_LIFETIME, appendValue, getLifetime());
APPEND_ENTITY_PROPERTY(PROP_SCRIPT, appendValue, getScript());
APPEND_ENTITY_PROPERTY(PROP_REGISTRATION_POINT, appendValue, getRegistrationPoint());
APPEND_ENTITY_PROPERTY(PROP_ANGULAR_VELOCITY, appendValue, getAngularVelocity());
APPEND_ENTITY_PROPERTY(PROP_ANGULAR_DAMPING, appendValue, getAngularDamping());
APPEND_ENTITY_PROPERTY(PROP_VISIBLE, appendValue, getVisible());
APPEND_ENTITY_PROPERTY(PROP_IGNORE_FOR_COLLISIONS, appendValue, getIgnoreForCollisions());
APPEND_ENTITY_PROPERTY(PROP_COLLISIONS_WILL_MOVE, appendValue, getCollisionsWillMove());
appendSubclassData(packetData, params, entityTreeElementExtraEncodeData,
requestedProperties,
propertyFlags,
propertiesDidntFit,
propertyCount,
appendState);
}
if (propertyCount > 0) {
int endOfEntityItemData = packetData->getUncompressedByteOffset();
encodedPropertyFlags = propertyFlags;
int newPropertyFlagsLength = encodedPropertyFlags.length();
packetData->updatePriorBytes(propertyFlagsOffset,
(const unsigned char*)encodedPropertyFlags.constData(), encodedPropertyFlags.length());
// if the size of the PropertyFlags shrunk, we need to shift everything down to front of packet.
if (newPropertyFlagsLength < oldPropertyFlagsLength) {
int oldSize = packetData->getUncompressedSize();
const unsigned char* modelItemData = packetData->getUncompressedData(propertyFlagsOffset + oldPropertyFlagsLength);
int modelItemDataLength = endOfEntityItemData - startOfEntityItemData;
int newEntityItemDataStart = propertyFlagsOffset + newPropertyFlagsLength;
packetData->updatePriorBytes(newEntityItemDataStart, modelItemData, modelItemDataLength);
int newSize = oldSize - (oldPropertyFlagsLength - newPropertyFlagsLength);
packetData->setUncompressedSize(newSize);
} else {
assert(newPropertyFlagsLength == oldPropertyFlagsLength); // should not have grown
}
packetData->endLevel(entityLevel);
} else {
packetData->discardLevel(entityLevel);
appendState = OctreeElement::NONE; // if we got here, then we didn't include the item
}
// If any part of the model items didn't fit, then the element is considered partial
if (appendState != OctreeElement::COMPLETED) {
// add this item into our list for the next appendElementData() pass
entityTreeElementExtraEncodeData->entities.insert(getEntityItemID(), propertiesDidntFit);
}
return appendState;
}
void Map::initPhysics ()
{
b2Vec2 gravity;
gravity.Set(0.0f, getGravity());
_world = new b2World(gravity);
_world->SetDestructionListener(&_destructionListener);
//_world->SetWarmStarting(false);
//_world->SetContinuousPhysics(false);
//_world->SetSubStepping(false);
_world->SetAutoClearForces(true);
_world->SetContactListener(this);
_world->SetContactFilter(this);
const float zeroX = 0.0f;
const float zeroY = -0.5f;
const float width = getMapWidth();
// added a small offset to allow water diving out of screen
const float height = getMapHeight() + 1.0f;
b2BodyDef lineBodyDef;
lineBodyDef.type = b2_staticBody;
lineBodyDef.position.Set(0, 0);
b2Body* boxBody = _world->CreateBody(&lineBodyDef);
b2EdgeShape edge;
b2FixtureDef fd;
fd.friction = 1.0f;
fd.restitution = 0.2f;
fd.shape = &edge;
_borders.resize(BORDER_MAX);
const bool isSideBorderFail = getSetting(msn::SIDEBORDERFAIL).toBool();
_borders[BORDER_TOP] = new Border(BorderType::TOP, *this);
_borders[BORDER_LEFT] = new Border(BorderType::LEFT, *this, isSideBorderFail);
_borders[BORDER_RIGHT] = new Border(BorderType::RIGHT, *this, isSideBorderFail);
_borders[BORDER_BOTTOM] = new Border(BorderType::BOTTOM, *this);
_borders[BORDER_PLAYER_BOTTOM] = new Border(BorderType::PLAYER_BOTTOM, *this);
edge.Set(b2Vec2(zeroX, zeroY), b2Vec2(width, zeroY));
b2Fixture *top = boxBody->CreateFixture(&fd);
top->SetUserData(_borders[BORDER_TOP]);
edge.Set(b2Vec2(zeroX, zeroY), b2Vec2(zeroX, height));
b2Fixture *left = boxBody->CreateFixture(&fd);
left->SetUserData(_borders[BORDER_LEFT]);
edge.Set(b2Vec2(width, height), b2Vec2(width, zeroY));
b2Fixture *right = boxBody->CreateFixture(&fd);
right->SetUserData(_borders[BORDER_RIGHT]);
edge.Set(b2Vec2(zeroX, height), b2Vec2(width, height));
b2Fixture *bottom = boxBody->CreateFixture(&fd);
bottom->SetUserData(_borders[BORDER_BOTTOM]);
edge.Set(b2Vec2(zeroX, height), b2Vec2(width, getMapHeight()));
b2Fixture *playerBottom = boxBody->CreateFixture(&fd);
playerBottom->SetUserData(_borders[BORDER_PLAYER_BOTTOM]);
initWater();
}
//Do the interpolation calculations
bool SIM_SnowSolver::solveGasSubclass(SIM_Engine &engine, SIM_Object *obj, SIM_Time time, SIM_Time framerate){
/// STEP #0: Retrieve all data objects from Houdini
//Scalar params
freal particle_mass = getPMass();
freal YOUNGS_MODULUS = getYoungsModulus();
freal POISSONS_RATIO = getPoissonsRatio();
freal CRIT_COMPRESS = getCritComp();
freal CRIT_STRETCH = getCritStretch();
freal FLIP_PERCENT = getFlipPercent();
freal HARDENING = getHardening();
freal CFL = getCfl();
freal COF = getCof();
freal division_size = getDivSize();
freal max_vel = getMaxVel();
//Vector params
vector3 GRAVITY = getGravity();
vector3 bbox_min_limit = getBboxMin();
vector3 bbox_max_limit = getBboxMax();
//Particle params
UT_String s_p, s_vol, s_den, s_vel, s_fe, s_fp;
getParticles(s_p);
getPVol(s_vol);
getPD(s_den);
getPVel(s_vel);
getPFe(s_fe);
getPFp(s_fp);
SIM_Geometry* geometry = (SIM_Geometry*) obj->getNamedSubData(s_p);
if (!geometry) return true;
//Get particle data
//Do we use the attribute name???
// GU_DetailHandle gdh = geometry->getGeometry().getWriteableCopy();
GU_DetailHandle gdh = geometry->getOwnGeometry();
const GU_Detail* gdp_in = gdh.readLock(); // Must unlock later
GU_Detail* gdp_out = gdh.writeLock();
GA_RWAttributeRef p_ref_position = gdp_out->findPointAttribute("P");
GA_RWHandleT<vector3> p_position(p_ref_position.getAttribute());
GA_RWAttributeRef p_ref_volume = gdp_out->findPointAttribute(s_vol);
GA_RWHandleT<freal> p_volume(p_ref_volume.getAttribute());
GA_RWAttributeRef p_ref_density = gdp_out->findPointAttribute(s_den);
GA_RWHandleT<freal> p_density(p_ref_density.getAttribute());
GA_RWAttributeRef p_ref_vel = gdp_out->findPointAttribute(s_vel);
GA_RWHandleT<vector3> p_vel(p_ref_vel.getAttribute());
GA_RWAttributeRef p_ref_Fe = gdp_out->findPointAttribute(s_fe);
GA_RWHandleT<matrix3> p_Fe(p_ref_Fe.getAttribute());
GA_RWAttributeRef p_ref_Fp = gdp_out->findPointAttribute(s_fp);
GA_RWHandleT<matrix3> p_Fp(p_ref_Fp.getAttribute());
//EVALUATE PARAMETERS
freal mu = YOUNGS_MODULUS/(2+2*POISSONS_RATIO);
freal lambda = YOUNGS_MODULUS*POISSONS_RATIO/((1+POISSONS_RATIO)*(1-2*POISSONS_RATIO));
//Get grid data
SIM_ScalarField *g_mass_field;
SIM_DataArray g_mass_data;
getMatchingData(g_mass_data, obj, MPM_G_MASS);
g_mass_field = SIM_DATA_CAST(g_mass_data(0), SIM_ScalarField);
SIM_VectorField *g_nvel_field;
SIM_DataArray g_nvel_data;
getMatchingData(g_nvel_data, obj, MPM_G_NVEL);
g_nvel_field = SIM_DATA_CAST(g_nvel_data(0), SIM_VectorField);
SIM_VectorField *g_ovel_field;
SIM_DataArray g_ovel_data;
getMatchingData(g_ovel_data, obj, MPM_G_OVEL);
g_ovel_field = SIM_DATA_CAST(g_ovel_data(0), SIM_VectorField);
SIM_ScalarField *g_active_field;
SIM_DataArray g_active_data;
getMatchingData(g_active_data, obj, MPM_G_ACTIVE);
g_active_field = SIM_DATA_CAST(g_active_data(0), SIM_ScalarField);
SIM_ScalarField *g_density_field;
SIM_DataArray g_density_data;
getMatchingData(g_density_data, obj, MPM_G_DENSITY);
g_density_field = SIM_DATA_CAST(g_density_data(0), SIM_ScalarField);
SIM_ScalarField *g_col_field;
SIM_DataArray g_col_data;
getMatchingData(g_col_data, obj, MPM_G_COL);
g_col_field = SIM_DATA_CAST(g_col_data(0), SIM_ScalarField);
SIM_VectorField *g_colVel_field;
SIM_DataArray g_colVel_data;
getMatchingData(g_colVel_data, obj, MPM_G_COLVEL);
g_colVel_field = SIM_DATA_CAST(g_colVel_data(0), SIM_VectorField);
SIM_VectorField *g_extForce_field;
SIM_DataArray g_extForce_data;
getMatchingData(g_extForce_data, obj, MPM_G_EXTFORCE);
g_extForce_field = SIM_DATA_CAST(g_extForce_data(0), SIM_VectorField);
UT_VoxelArrayF
*g_mass = g_mass_field->getField()->fieldNC(),
*g_nvelX = g_nvel_field->getField(0)->fieldNC(),
*g_nvelY = g_nvel_field->getField(1)->fieldNC(),
*g_nvelZ = g_nvel_field->getField(2)->fieldNC(),
*g_ovelX = g_ovel_field->getField(0)->fieldNC(),
*g_ovelY = g_ovel_field->getField(1)->fieldNC(),
*g_ovelZ = g_ovel_field->getField(2)->fieldNC(),
*g_colVelX = g_colVel_field->getField(0)->fieldNC(),
*g_colVelY = g_colVel_field->getField(1)->fieldNC(),
*g_colVelZ = g_colVel_field->getField(2)->fieldNC(),
*g_extForceX = g_extForce_field->getField(0)->fieldNC(),
*g_extForceY = g_extForce_field->getField(1)->fieldNC(),
*g_extForceZ = g_extForce_field->getField(2)->fieldNC(),
*g_col = g_col_field->getField()->fieldNC(),
*g_active = g_active_field->getField()->fieldNC();
int point_count = gdp_out->getPointRange().getEntries();
std::vector<boost::array<freal,64> > p_w(point_count);
std::vector<boost::array<vector3,64> > p_wgh(point_count);
//Get world-to-grid conversion ratios
//Particle's grid position can be found via (pos - grid_origin)/voxel_dims
vector3
voxel_dims = g_mass_field->getVoxelSize(),
grid_origin = g_mass_field->getOrig(),
grid_divs = g_mass_field->getDivisions();
//Houdini uses voxel centers for grid nodes, rather than grid corners
grid_origin += voxel_dims/2.0;
freal voxelArea = voxel_dims[0]*voxel_dims[1]*voxel_dims[2];
/*
//Reset grid
for(int iX=0; iX < grid_divs[0]; iX++){
for(int iY=0; iY < grid_divs[1]; iY++){
for(int iZ=0; iZ < grid_divs[2]; iZ++){
g_mass->setValue(iX,iY,iZ,0);
g_active->setValue(iX,iY,iZ,0);
g_ovelX->setValue(iX,iY,iZ,0);
g_ovelY->setValue(iX,iY,iZ,0);
g_ovelZ->setValue(iX,iY,iZ,0);
g_nvelX->setValue(iX,iY,iZ,0);
g_nvelY->setValue(iX,iY,iZ,0);
g_nvelZ->setValue(iX,iY,iZ,0);
}
}
}
*/
/// STEP #1: Transfer mass to grid
if (p_position.isValid()){
//Iterate through particles
for (GA_Iterator it(gdp_out->getPointRange()); !it.atEnd(); it.advance()){
int pid = it.getOffset();
//Get grid position
vector3 gpos = (p_position.get(pid) - grid_origin)/voxel_dims;
int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2];
//g_mass_field->posToIndex(p_position.get(pid),p_gridx,p_gridy,p_gridz);
freal particle_density = p_density.get(pid);
//Compute weights and transfer mass
for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){
//Z-dimension interpolation
freal z_pos = gpos[2]-z,
wz = SIM_SnowSolver::bspline(z_pos),
dz = SIM_SnowSolver::bsplineSlope(z_pos);
for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){
//Y-dimension interpolation
freal y_pos = gpos[1]-y,
wy = SIM_SnowSolver::bspline(y_pos),
dy = SIM_SnowSolver::bsplineSlope(y_pos);
for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){
//X-dimension interpolation
freal x_pos = gpos[0]-x,
wx = SIM_SnowSolver::bspline(x_pos),
dx = SIM_SnowSolver::bsplineSlope(x_pos);
//Final weight is dyadic product of weights in each dimension
freal weight = wx*wy*wz;
p_w[pid-1][idx] = weight;
//Weight gradient is a vector of partial derivatives
p_wgh[pid-1][idx] = vector3(dx*wy*wz, wx*dy*wz, wx*wy*dz)/voxel_dims;
//Interpolate mass
freal node_mass = g_mass->getValue(x,y,z);
node_mass += weight*particle_mass;
g_mass->setValue(x,y,z,node_mass);
}
}
}
}
}
/// STEP #2: First timestep only - Estimate particle volumes using grid mass
/*
if (time == 0.0){
//Iterate through particles
for (GA_Iterator it(gdp_out->getPointRange()); !it.atEnd(); it.advance()){
int pid = it.getOffset();
freal density = 0;
//Get grid position
int p_gridx = 0, p_gridy = 0, p_gridz = 0;
vector3 gpos = (p_position.get(pid) - grid_origin)/voxel_dims;
int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2];
//g_nvel_field->posToIndex(0,p_position.get(pid),p_gridx,p_gridy,p_gridz);
//Transfer grid density (within radius) to particles
for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){
for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){
for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){
freal w = p_w[pid-1][idx];
if (w > EPSILON){
//Transfer density
density += w * g_mass->getValue(x,y,z);
}
}
}
}
density /= voxelArea;
p_density.set(pid,density);
p_volume.set(pid, particle_mass/density);
}
}
//*/
/// STEP #3: Transfer velocity to grid
//This must happen after transferring mass, to conserve momentum
//Iterate through particles and transfer
for (GA_Iterator it(gdp_in->getPointRange()); !it.atEnd(); it.advance()){
int pid = it.getOffset();
vector3 vel_fac = p_vel.get(pid)*particle_mass;
//Get grid position
vector3 gpos = (p_position.get(pid) - grid_origin)/voxel_dims;
int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2];
//g_nvel_field->posToIndex(0,p_position.get(pid),p_gridx,p_gridy,p_gridz);
//Transfer to grid nodes within radius
for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){
for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){
for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){
freal w = p_w[pid-1][idx];
if (w > EPSILON){
freal nodex_vel = g_ovelX->getValue(x,y,z) + vel_fac[0]*w;
freal nodey_vel = g_ovelY->getValue(x,y,z) + vel_fac[1]*w;
freal nodez_vel = g_ovelZ->getValue(x,y,z) + vel_fac[2]*w;
g_ovelX->setValue(x,y,z,nodex_vel);
g_ovelY->setValue(x,y,z,nodey_vel);
g_ovelZ->setValue(x,y,z,nodez_vel);
g_active->setValue(x,y,z,1.0);
}
}
}
}
}
//Division is slow (maybe?); we only want to do divide by mass once, for each active node
for(int iX=0; iX < grid_divs[0]; iX++){
for(int iY=0; iY < grid_divs[1]; iY++){
for(int iZ=0; iZ < grid_divs[2]; iZ++){
//Only check nodes that have mass
if (g_active->getValue(iX,iY,iZ)){
freal node_mass = 1/(g_mass->getValue(iX,iY,iZ));
g_ovelX->setValue(iX,iY,iZ,(g_ovelX->getValue(iX,iY,iZ)*node_mass));
g_ovelY->setValue(iX,iY,iZ,(g_ovelY->getValue(iX,iY,iZ)*node_mass));
g_ovelZ->setValue(iX,iY,iZ,(g_ovelZ->getValue(iX,iY,iZ)*node_mass));
}
}
}
}
/// STEP #4: Compute new grid velocities
//Temporary variables for plasticity and force calculation
//We need one set of variables for each thread that will be running
eigen_matrix3 def_elastic, def_plastic, energy, svd_u, svd_v;
Eigen::JacobiSVD<eigen_matrix3, Eigen::NoQRPreconditioner> svd;
eigen_vector3 svd_e;
matrix3 HDK_def_plastic, HDK_def_elastic, HDK_energy;
freal* data_dp = HDK_def_plastic.data();
freal* data_de = HDK_def_elastic.data();
freal* data_energy = HDK_energy.data();
//Map Eigen matrices to HDK matrices
Eigen::Map<eigen_matrix3> data_dp_map(data_dp);
Eigen::Map<eigen_matrix3> data_de_map(data_de);
Eigen::Map<eigen_matrix3> data_energy_map(data_energy);
//Compute force at each particle and transfer to Eulerian grid
//We use "nvel" to hold the grid force, since that variable is not in use
for (GA_Iterator it(gdp_in->getPointRange()); !it.atEnd(); it.advance()){
int pid = it.getOffset();
//Apply plasticity to deformation gradient, before computing forces
//We need to use the Eigen lib to do the SVD; transfer houdini matrices to Eigen matrices
HDK_def_plastic = p_Fp.get(pid);
HDK_def_elastic = p_Fe.get(pid);
def_plastic = Eigen::Map<eigen_matrix3>(data_dp);
def_elastic = Eigen::Map<eigen_matrix3>(data_de);
//Compute singular value decomposition (uev*)
svd.compute(def_elastic, Eigen::ComputeFullV | Eigen::ComputeFullU);
svd_e = svd.singularValues();
svd_u = svd.matrixU();
svd_v = svd.matrixV();
//Clamp singular values
for (int i=0; i<3; i++){
if (svd_e[i] < CRIT_COMPRESS)
svd_e[i] = CRIT_COMPRESS;
else if (svd_e[i] > CRIT_STRETCH)
svd_e[i] = CRIT_STRETCH;
}
//Put SVD back together for new elastic and plastic gradients
def_plastic = svd_v * svd_e.asDiagonal().inverse() * svd_u.transpose() * def_elastic * def_plastic;
svd_v.transposeInPlace();
def_elastic = svd_u * svd_e.asDiagonal() * svd_v;
//Now compute the energy partial derivative (which we use to get force at each grid node)
energy = 2*mu*(def_elastic - svd_u*svd_v)*def_elastic.transpose();
//Je is the determinant of def_elastic (equivalent to svd_e.prod())
freal Je = svd_e.prod(),
contour = lambda*Je*(Je-1),
jp = def_plastic.determinant(),
particle_vol = p_volume.get(pid);
for (int i=0; i<3; i++)
energy(i,i) += contour;
energy *= particle_vol * exp(HARDENING*(1-jp));
//Transfer Eigen matrices back to HDK
data_dp_map = def_plastic;
data_de_map = def_elastic;
data_energy_map = energy;
p_Fp.set(pid,HDK_def_plastic);
p_Fe.set(pid,HDK_def_elastic);
//Transfer energy to surrounding grid nodes
vector3 gpos = (p_position.get(pid) - grid_origin)/voxel_dims;
int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2];
for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){
for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){
for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){
freal w = p_w[pid-1][idx];
if (w > EPSILON){
vector3 ngrad = p_wgh[pid-1][idx];
g_nvelX->setValue(x,y,z,g_nvelX->getValue(x,y,z) + ngrad.dot(HDK_energy[0]));
g_nvelY->setValue(x,y,z,g_nvelY->getValue(x,y,z) + ngrad.dot(HDK_energy[1]));
g_nvelZ->setValue(x,y,z,g_nvelZ->getValue(x,y,z) + ngrad.dot(HDK_energy[2]));
}
}
}
}
}
//Use new forces to solve for new velocities
for(int iX=0; iX < grid_divs[0]; iX++){
for(int iY=0; iY < grid_divs[1]; iY++){
for(int iZ=0; iZ < grid_divs[2]; iZ++){
//Only compute for active nodes
if (g_active->getValue(iX,iY,iZ)){
freal nodex_ovel = g_ovelX->getValue(iX,iY,iZ);
freal nodey_ovel = g_ovelY->getValue(iX,iY,iZ);
freal nodez_ovel = g_ovelZ->getValue(iX,iY,iZ);
freal forcex = g_nvelX->getValue(iX,iY,iZ);
freal forcey = g_nvelY->getValue(iX,iY,iZ);
freal forcez = g_nvelZ->getValue(iX,iY,iZ);
freal node_mass = 1/(g_mass->getValue(iX,iY,iZ));
freal ext_forceX = GRAVITY[0] + g_extForceX->getValue(iX,iY,iZ);
freal ext_forceY = GRAVITY[1] + g_extForceY->getValue(iX,iY,iZ);
freal ext_forceZ = GRAVITY[2] + g_extForceZ->getValue(iX,iY,iZ);
nodex_ovel += framerate*(ext_forceX - forcex*node_mass);
nodey_ovel += framerate*(ext_forceY - forcey*node_mass);
nodez_ovel += framerate*(ext_forceZ - forcez*node_mass);
//Limit velocity to max_vel
vector3 g_nvel(nodex_ovel, nodey_ovel, nodez_ovel);
freal nvelNorm = g_nvel.length();
if(nvelNorm > max_vel){
freal velRatio = max_vel/nvelNorm;
g_nvel*= velRatio;
}
g_nvelX->setValue(iX,iY,iZ,g_nvel[0]);
g_nvelY->setValue(iX,iY,iZ,g_nvel[1]);
g_nvelZ->setValue(iX,iY,iZ,g_nvel[2]);
}
}
}
}
/// STEP #5: Grid collision resolution
vector3 sdf_normal;
//*
for(int iX=1; iX < grid_divs[0]-1; iX++){
for(int iY=1; iY < grid_divs[1]-1; iY++){
for(int iZ=1; iZ < grid_divs[2]-1; iZ++){
if (g_active->getValue(iX,iY,iZ)){
if (!computeSDFNormal(g_col, iX, iY, iZ, sdf_normal))
continue;
//Collider velocity
vector3 vco(
g_colVelX->getValue(iX,iY,iZ),
g_colVelY->getValue(iX,iY,iZ),
g_colVelZ->getValue(iX,iY,iZ)
);
//Grid velocity
vector3 v(
g_nvelX->getValue(iX,iY,iZ),
g_nvelY->getValue(iX,iY,iZ),
g_nvelZ->getValue(iX,iY,iZ)
);
//Skip if bodies are separating
vector3 vrel = v - vco;
freal vn = vrel.dot(sdf_normal);
if (vn >= 0) continue;
//Resolve collisions; also add velocity of collision object to snow velocity
//Sticks to surface (too slow to overcome static friction)
vector3 vt = vrel - (sdf_normal*vn);
freal stick = vn*COF, vt_norm = vt.length();
if (vt_norm <= -stick)
vt = vco;
//Dynamic friction
else vt += stick*vt/vt_norm + vco;
g_nvelX->setValue(iX,iY,iZ,vt[0]);
g_nvelY->setValue(iX,iY,iZ,vt[1]);
g_nvelZ->setValue(iX,iY,iZ,vt[2]);
}
}
}
}
//*/
/// STEP #6: Transfer grid velocities to particles and integrate
/// STEP #7: Particle collision resolution
vector3 pic, flip, col_vel;
matrix3 vel_grad;
//Iterate through particles
for (GA_Iterator it(gdp_in->getPointRange()); !it.atEnd(); it.advance()){
int pid = it.getOffset();
//Particle position
vector3 pos(p_position.get(pid));
//Reset velocity
pic[0] = 0.0;
pic[1] = 0.0;
pic[2] = 0.0;
flip = p_vel.get(pid);
vel_grad.zero();
freal density = 0;
//Get grid position
vector3 gpos = (pos - grid_origin)/voxel_dims;
int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2];
for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){
for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){
for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){
freal w = p_w[pid-1][idx];
if (w > EPSILON){
const vector3 node_wg = p_wgh[pid-1][idx];
const vector3 node_nvel(
g_nvelX->getValue(x,y,z),
g_nvelY->getValue(x,y,z),
g_nvelZ->getValue(x,y,z)
);
//Transfer velocities
pic += node_nvel*w;
flip[0] += (node_nvel[0] - g_ovelX->getValue(x,y,z))*w;
flip[1] += (node_nvel[1]- g_ovelY->getValue(x,y,z))*w;
flip[2] += (node_nvel[2] - g_ovelZ->getValue(x,y,z))*w;
//Transfer density
density += w * g_mass->getValue(x,y,z);
//Transfer veloctiy gradient
vel_grad.outerproductUpdate(1.0, node_nvel, node_wg);
}
}
}
}
//Finalize velocity update
vector3 vel = flip*FLIP_PERCENT + pic*(1-FLIP_PERCENT);
//Reset collision data
freal col_sdf = 0;
sdf_normal[0] = 0;
sdf_normal[1] = 0;
sdf_normal[2] = 0;
col_vel[0] = 0;
col_vel[1] = 0;
col_vel[2] = 0;
//Interpolate surrounding nodes' SDF info to the particle (trilinear interpolation)
for (int idx=0, z=p_gridz, z_end=z+1; z<=z_end; z++){
freal w_z = gpos[2]-z;
for (int y=p_gridy, y_end=y+1; y<=y_end; y++){
freal w_zy = w_z*(gpos[1]-y);
for (int x=p_gridx, x_end=x+1; x<=x_end; x++, idx++){
freal weight = fabs(w_zy*(gpos[0]-x));
//cout << w_zy << "," << (gpos[0]-x) << "," << weight << endl;
vector3 temp_normal;
computeSDFNormal(g_col, x, y, z, temp_normal);
//goto SKIP_PCOLLIDE;
//cout << g_col->getValue(x, y, z) << endl;
//Interpolate
sdf_normal += temp_normal*weight;
col_sdf += g_col->getValue(x, y, z)*weight;
col_vel[0] += g_colVelX->getValue(x, y, z)*weight;
col_vel[1] += g_colVelY->getValue(x, y, z)*weight;
col_vel[2] += g_colVelZ->getValue(x, y, z)*weight;
}
}
}
//Resolve particle collisions
//cout << col_sdf << endl;
if (col_sdf > 0){
vector3 vrel = vel - col_vel;
freal vn = vrel.dot(sdf_normal);
//Skip if bodies are separating
if (vn < 0){
//Resolve and add velocity of collision object to snow velocity
//Sticks to surface (too slow to overcome static friction)
vel = vrel - (sdf_normal*vn);
freal stick = vn*COF, vel_norm = vel.length();
if (vel_norm <= -stick)
vel = col_vel;
//Dynamic friction
else vel += stick*vel/vel_norm + col_vel;
}
}
SKIP_PCOLLIDE:
//Finalize density update
density /= voxelArea;
p_density.set(pid,density);
//Update particle position
pos += framerate*vel;
//Limit particle position
int mask = 0;
/*
for (int i=0; i<3; i++){
if (pos[i] > bbox_max_limit[i]){
pos[i] = bbox_max_limit[i];
vel[i] = 0.0;
mask |= 1 << i;
}
else if (pos[i] < bbox_min_limit[i]){
pos[i] = bbox_min_limit[i];
vel[i] = 0.0;
mask |= 1 << i;
}
}
//Slow particle down at bounds (not really necessary...)
if (mask){
for (int i=0; i<3; i++){
if (mask & 0x1)
vel[i] *= .05;
mask >>= 1;
}
}//*/
p_vel.set(pid,vel);
p_position.set(pid,pos);
//Update particle deformation gradient
//Note: plasticity is computed on the next timestep...
vel_grad *= framerate;
vel_grad(0,0) += 1;
vel_grad(1,1) += 1;
vel_grad(2,2) += 1;
p_Fe.set(pid, vel_grad*p_Fe.get(pid));
}
gdh.unlock(gdp_out);
gdh.unlock(gdp_in);
return true;
}
