void Transform::setPosition(glm::vec3 position_){ assert(!glm::any(glm::isnan(position_))); if (!mParent){ setLocalPosition(position_); return; } vec3 currentPosition = position(); setLocalPosition(currentPosition - position_); markLocalDirty(); }
void Transform::setParent(Transform* transform) { if(this->parent != NULL) { for(int i = 0; i < this->parent->children.size(); i++) { if(this->parent->children.at(i) == this) { this->parent->children.erase(this->parent->children.begin() + i); break; } } } if(transform != NULL) { transform->children.push_back(this); } setLocalPosition(getPosition()); setLocalRotation(getRotation()); this->parent = transform; setPosition(getLocalPosition()); setRotation(getLocalRotation()); }
bool VPathNode::fromString( const String &pString ) { // Split Data. // {Position} {Rotation} {Weight} const char *baseData = StringUnit::getUnit( pString.c_str(), 0, "\t" ); Point3F pos; AngAxisF aa; F32 weight; // Scan Base. dSscanf( baseData, "%g %g %g %g %g %g %g %g", &pos.x, &pos.y, &pos.z, &aa.axis.x, &aa.axis.y, &aa.axis.z, &aa.angle, &weight ); // Apply Changes. setLocalPosition( pos ); setLocalRotation( QuatF( aa ) ); setWeight( weight ); // Fetch Orientation Data. String orientationData = StringUnit::getUnit( pString.c_str(), 1, "\t" ); // Fetch Orientation Type. String orientationTypeString = orientationData; if ( orientationData.find( " " ) ) { // Use First Word. orientationTypeString = orientationData.substr( 0, orientationData.find( " " ) ); } // Set Orientation Type. const eOrientationType &orientationType = getOrientationTypeEnum( orientationTypeString.c_str() ); switch( orientationType ) { case k_OrientationFree : { // Apply Mode. setOrientationMode( orientationType ); } break; case k_OrientationToPoint: { // Fetch Point. Point3F lookAtPoint; // Buffer String. dSscanf( orientationData.c_str(), "%*s %f %f %f", &lookAtPoint.x, &lookAtPoint.y, &lookAtPoint.z ); // Apply Mode. setOrientationMode( orientationType, lookAtPoint ); } break; } return true; }
TEST (RigidRepresentationContactTests, SetGet_BuildMlcp_Test) { Vector3d n(0.0, 1.0, 0.0); double d = 0.0; double radius = 0.01; double violation = -radius; Vector3d contactPosition = -n * (d - violation); SurgSim::Math::RigidTransform3d poseRigid; poseRigid.setIdentity(); std::shared_ptr<RigidRepresentation> rigid = std::make_shared<RigidRepresentation>("Rigid"); rigid->setLocalActive(true); rigid->setIsGravityEnabled(false); rigid->setLocalPose(poseRigid); rigid->setDensity(1000.0); rigid->setShape(std::make_shared<SphereShape>(radius)); auto loc = std::make_shared<RigidRepresentationLocalization>(rigid); loc->setLocalPosition(contactPosition); std::shared_ptr<RigidRepresentationContact> implementation = std::make_shared<RigidRepresentationContact>(); EXPECT_EQ(SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT, implementation->getMlcpConstraintType()); EXPECT_EQ(1u, implementation->getNumDof()); ContactConstraintData constraintData; constraintData.setPlaneEquation(n, d); MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(rigid->getNumDof(), 1, 1); // Fill up the Mlcp double dt = 1e-3; implementation->build(dt, constraintData, loc, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE); // Violation b should be exactly violation = -radius (the sphere center is on the plane) EXPECT_NEAR(violation, mlcpPhysicsProblem.b[0], epsilon); // Constraint H should be // H = dt.[nx ny nz nz.GPy-ny.GPz nx.GPz-nz.GPx ny.GPx-nx.GPy] Vector3d n_GP = n.cross(Vector3d(0.0, 0.0, 0.0)); SurgSim::Math::Matrix h = mlcpPhysicsProblem.H; EXPECT_NEAR(dt * n[0] , h(0, 0), epsilon); EXPECT_NEAR(dt * n[1] , h(0, 1), epsilon); EXPECT_NEAR(dt * n[2] , h(0, 2), epsilon); EXPECT_NEAR(dt * n_GP[0], h(0, 3), epsilon); EXPECT_NEAR(dt * n_GP[1], h(0, 4), epsilon); EXPECT_NEAR(dt * n_GP[2], h(0, 5), epsilon); // ConstraintTypes should contain 0 entry as it is setup by the constraint and not the ConstraintImplementation // This way, the constraint can verify that both ConstraintImplementation are the same type ASSERT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size()); }
TEST (FixedConstraintFrictionlessContactTests, SetGet_BuildMlcp_Test) { SurgSim::Math::Vector3d n(0.0, 1.0, 0.0); double d = 0.0; double violation = -0.01; SurgSim::Math::Vector3d contactPosition = -n * (d - violation); SurgSim::Math::RigidTransform3d poseFixed; poseFixed.setIdentity(); std::shared_ptr<FixedRepresentation> fixed = std::make_shared<FixedRepresentation>("Fixed"); fixed->setLocalActive(true); fixed->setIsGravityEnabled(false); fixed->setLocalPose(poseFixed); auto loc = std::make_shared<FixedLocalization>(fixed); loc->setLocalPosition(contactPosition); std::shared_ptr<FixedConstraintFrictionlessContact> implementation = std::make_shared<FixedConstraintFrictionlessContact>(); EXPECT_EQ(SurgSim::Physics::FRICTIONLESS_3DCONTACT, implementation->getConstraintType()); EXPECT_EQ(1u, implementation->getNumDof()); ContactConstraintData constraintData; constraintData.setPlaneEquation(n, d); MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(fixed->getNumDof(), 1, 1); // Fill up the Mlcp double dt = 1e-3; implementation->build(dt, constraintData, loc, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE); // b should be exactly the violation EXPECT_NEAR(violation, mlcpPhysicsProblem.b[0], epsilon); // Constraint H should be [] (a fixed representation has no dof !) // ConstraintTypes should contain 0 entry as it is setup by the constraint and not the ConstraintImplementation // This way, the constraint can verify that both ConstraintImplementation are the same type ASSERT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size()); }
////////////////////////////////////////////////////////////////////////// // update //virtual void GaRobotComponent::update( BcF32 Tick ) { if( Health_ <= 0.0f ) { return; } CurrentOpTimer_ -= Tick; if( CurrentOpTimer_ < 0.0f ) { CurrentOpTimer_ += CurrentOpTime_; // Handle robot program. BcBool ExecutedCode = BcFalse; if( Program_.size() > 0 ) { CurrentOp_ = NextOp_; const auto& Op = Program_[ CurrentOp_ ]; if( Op.State_ == CurrentState_ ) { auto Condition = ProgramFunctionMap_[ Op.Condition_ ]; if( Condition != nullptr ) { if( Condition( this, Op.ConditionVar_ ) ) { auto Operation = ProgramFunctionMap_[ Op.Operation_ ]; if( Operation == nullptr ) { BcPrintf( "No operation \"%s\"\n", Op.Operation_.c_str() ); } else { auto RetVal = Operation( this, Op.OperationVar_ ); if( RetVal != BcErrorCode ) { CurrentState_ = RetVal; } } } } ExecutedCode = BcTrue; } // Advance to next valid op. if( ExecutedCode ) { for( BcU32 Idx = 0; Idx < Program_.size(); ++Idx ) { NextOp_ = ( NextOp_ + 1 ) % Program_.size(); if( Program_[ NextOp_ ].State_ == CurrentState_ ) { break; } } } // Did we fail to run code? If so, reset to op 0 and the state of op 0. if( ExecutedCode == BcFalse ) { NextOp_ = 0; CurrentState_ = Program_[ NextOp_ ].State_; } } } // Grab entity + position. auto Entity = getParentEntity(); auto LocalPosition = Entity->getLocalPosition(); // Move if we need to move towards our target position. if( ( TargetPosition_ - LocalPosition ).magnitudeSquared() > ( TargetDistance_ * TargetDistance_ ) ) { if( MoveTimer_ <= 0.0f ) { Velocity_ += ( TargetPosition_ - LocalPosition ).normal() * MaxVelocity_; } } else { BcF32 SlowDownTick = BcClamp( Tick * 50.0f, 0.0f, 1.0f ); Velocity_ -= ( Velocity_ * SlowDownTick ); } // TODO LATER: Do rotation. if( Velocity_.magnitudeSquared() > 0.1f ) { auto Angle = std::atan2( Velocity_.z(), Velocity_.x() ) + BcPIDIV2; MaMat4d RotMat; RotMat.rotation( MaVec3d( 0.0f, Angle, 0.0f ) ); Base_->setLocalMatrix( RotMat ); } // TODO LATER: Do rotation. auto Robots = getRobots( 1 - Team_ ); if( Robots.size() > 0 ) { auto Robot = Robots[ 0 ]; auto RobotPosition = Robot->getParentEntity()->getLocalPosition(); auto VectorTo = RobotPosition - LocalPosition; // Push out of away. if( VectorTo.magnitude() < 3.0f ) { BcF32 Factor = ( 3.0f - VectorTo.magnitude() ) / 3.0f; BcF32 InvFactor = 1.0f - Factor; Velocity_ = ( -( VectorTo.normal() * MaxVelocity_ ) * Factor * 3.0f ) + ( Velocity_ * InvFactor ); } // Face turret. auto Angle = std::atan2( VectorTo.z(), VectorTo.x() ) + BcPIDIV2; MaMat4d RotMat; RotMat.rotation( MaVec3d( 0.0f, Angle, 0.0f ) ); Turret_->setLocalMatrix( RotMat ); } LocalPosition += Velocity_ * Tick; // Slow down velocity. BcF32 SlowDownTick = BcClamp( Tick * 10.0f, 0.0f, 1.0f ); Velocity_ -= ( Velocity_ * SlowDownTick ); if( Velocity_.magnitude() > MaxVelocity_ ) { Velocity_ = Velocity_.normal() * MaxVelocity_; } // Set local position. Entity->setLocalPosition( LocalPosition ); // Handle health + energy. Health_ = BcClamp( Health_, 0.0f, 100.0f ); Energy_ = BcClamp( Energy_ + ( EnergyChargeRate_ * Tick ), 0.0f, 100.0f ); // Weapon timers. WeaponATimer_ = BcMax( WeaponATimer_ - Tick, -1.0f ); WeaponBTimer_ = BcMax( WeaponBTimer_ - Tick, -1.0f ); MoveTimer_ = BcMax( MoveTimer_ - Tick, -1.0f ); // Health/energy bars. OsClient* Client = OsCore::pImpl()->getClient( 0 ); BcF32 Width = BcF32( Client->getWidth() ) * 0.5f; BcF32 Height = BcF32( Client->getHeight() ) * 0.5f; MaMat4d Projection; Projection.orthoProjection( -Width, Width, Height, -Height, -1.0f, 1.0f ); Canvas_->pushMatrix( Projection ); Canvas_->setMaterialComponent( Material_ ); auto ScreenPos = View_->getScreenPosition( getParentEntity()->getWorldPosition() ); ScreenPos -= MaVec2d( 0.0f, Height / 8.0f ); auto TLPos = ScreenPos - MaVec2d( Width / 16.0f, Height / 64.0f ); auto BRPos = ScreenPos + MaVec2d( Width / 16.0f, Height / 64.0f ); // Draw background. Canvas_->drawBox( TLPos, BRPos, RsColour::BLACK, 0 ); // Draw inner bars. TLPos += MaVec2d( 1.0f, 1.0f ); BRPos -= MaVec2d( 1.0f, 1.0f ); auto HealthTL = MaVec2d( TLPos.x(), TLPos.y() ); auto HealthBR = MaVec2d( TLPos.x() + ( BRPos.x() - TLPos.x() ) * ( Health_ / 100.0f ), ( TLPos.y() + BRPos.y() ) * 0.5f ); auto EnergyTL = MaVec2d( TLPos.x(), ( TLPos.y() + BRPos.y() ) * 0.5f ); auto EnergyBR = MaVec2d( TLPos.x() + ( BRPos.x() - TLPos.x() ) * ( Energy_ / 100.0f ), BRPos.y() ); Canvas_->drawBox( HealthTL, HealthBR, RsColour::GREEN, 0 ); Canvas_->drawBox( EnergyTL, EnergyBR, RsColour::BLUE, 0 ); Canvas_->popMatrix( ); ScnDebugRenderComponent::pImpl()->drawLine( LocalPosition, TargetPosition_, RsColour::WHITE, 0 ); Super::update( Tick ); }
void PUParticleSystem3D::processParticle( ParticlePool &pool, bool &firstActiveParticle, bool &firstParticle, float elapsedTime ) { PUParticle3D *particle = static_cast<PUParticle3D *>(pool.getFirst()); //Mat4 ltow = getNodeToWorldTransform(); //Vec3 scl; //Quaternion rot; //ltow.decompose(&scl, &rot, nullptr); while (particle){ if (!isExpired(particle, elapsedTime)){ particle->process(elapsedTime); //if (_emitter && _emitter->isEnabled()) // _emitter->updateEmitter(particle, elapsedTime); for (auto it : _emitters) { if (it->isEnabled() && !it->isMarkedForEmission()){ (static_cast<PUEmitter*>(it))->updateEmitter(particle, elapsedTime); } } for (auto& it : _affectors) { if (it->isEnabled()){ (static_cast<PUAffector*>(it))->process(particle, elapsedTime, firstActiveParticle); } } if (_render) static_cast<PURender *>(_render)->updateRender(particle, elapsedTime, firstActiveParticle); if (_isEnabled && particle->particleType != PUParticle3D::PT_VISUAL){ if (particle->particleType == PUParticle3D::PT_EMITTER){ auto emitter = static_cast<PUEmitter *>(particle->particleEntityPtr); emitter->setLocalPosition(particle->position); executeEmitParticles(emitter, emitter->calculateRequestedParticles(elapsedTime), elapsedTime); }else if (particle->particleType == PUParticle3D::PT_TECHNIQUE){ auto system = static_cast<PUParticleSystem3D *>(particle->particleEntityPtr); system->setPosition3D(particle->position); system->setRotationQuat(particle->orientation); //system->setScaleX(scl.x);system->setScaleY(scl.y);system->setScaleZ(scl.z); system->forceUpdate(elapsedTime); } } firstActiveParticle = false; // Keep latest position particle->latestPosition = particle->position; //if (_maxVelocitySet && particle->calculateVelocity() > _maxVelocity) //{ // particle->direction *= (_maxVelocity / particle->direction.length()); //} //// Update the position with the direction. //particle->position += (particle->direction * _particleSystemScaleVelocity * elapsedTime); //particle->positionInWorld = particle->position; //particle->orientationInWorld = particle->orientation; //particle->widthInWorld = particle->width; //particle->heightInWorld = particle->height; //particle->depthInWorld = particle->depth; //bool keepLocal = _keepLocal; //PUParticleSystem3D *parent = dynamic_cast<PUParticleSystem3D *>(getParent()); //if (parent) keepLocal = keepLocal || parent->isKeepLocal(); //if (keepLocal){ // ltow.transformPoint(particle->positionInWorld, &particle->positionInWorld); // Vec3 ori; // ltow.transformVector(Vec3(particle->orientation.x, particle->orientation.y, particle->orientation.z), &ori); // particle->orientationInWorld.x = ori.x; particle->orientationInWorld.y = ori.y; particle->orientationInWorld.z = ori.z; // particle->widthInWorld = scl.x * particle->width; // particle->heightInWorld = scl.y * particle->height; // particle->depthInWorld = scl.z * particle->depth; //} processMotion(particle, elapsedTime, firstActiveParticle); } else{ initParticleForExpiration(particle, elapsedTime); pool.lockLatestData(); } for (auto it : _observers){ if (it->isEnabled()){ it->updateObserver(particle, elapsedTime, firstParticle); } } if (particle->hasEventFlags(PUParticle3D::PEF_EXPIRED)) { particle->setEventFlags(0); particle->addEventFlags(PUParticle3D::PEF_EXPIRED); } else { particle->setEventFlags(0); } particle->timeToLive -= elapsedTime; firstParticle = false; particle = static_cast<PUParticle3D *>(pool.getNext()); } }
//-- Constructor UIObjectSliced::UIObjectSliced(char* name, char* imagePath, GLuint width, GLuint height, GLint xPos, GLint yPos, GLuint program) { new(this) UIObjectSliced(name, imagePath, width, height, program); setLocalPosition(xPos, yPos); }
void UIView::setLocalPosition(vec2 localPosition) { setLocalPosition(localPosition.x, localPosition.y); }