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);
}