void PhysicsModule::update(sf::Time deltaT)
{
float t = 1;
//C
float c = t*t*.5;
setAccel(getAccel() * c);
//B
setVel(getVel() * t);
//B&C
addVel(getAccel());
//A&B&C
addPos(getVel());
setAccel(sf::Vector2f(0,0));
}
bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
if (index < atoms_.size()) {
Vector3d velRot;
Mat3x3d skewMat;;
Vector3d ref = refCoords_[index];
Vector3d ji = getJ();
Mat3x3d I = getI();
skewMat(0, 0) =0;
skewMat(0, 1) = ji[2] /I(2, 2);
skewMat(0, 2) = -ji[1] /I(1, 1);
skewMat(1, 0) = -ji[2] /I(2, 2);
skewMat(1, 1) = 0;
skewMat(1, 2) = ji[0]/I(0, 0);
skewMat(2, 0) =ji[1] /I(1, 1);
skewMat(2, 1) = -ji[0]/I(0, 0);
skewMat(2, 2) = 0;
velRot = (getA() * skewMat).transpose() * ref;
vel =getVel() + velRot;
return true;
} else {
std::cerr << index << " is an invalid index, current rigid body contains "
<< atoms_.size() << "atoms" << std::endl;
return false;
}
}
int main(){
double dt = 0.1;
double *time;
double t_start, t_end;
int time_steps, Nsamples;
Matrix *xSaved, *zSaved;
double z;
t_start = 0;
t_end = 10;
time_steps =(int)((double)(t_end - t_start)/dt + 0.5);
//arredonda para cima o resultado da divisão
time = (double *)aloca(time_steps*sizeof(double));
int i;
for(i = 0; i < time_steps; i++){
time[i] = i*dt;
}
Nsamples = time_steps;
xSaved = new_M(Nsamples, 3);
zSaved = new_M(Nsamples, 2);
for(i = 0; i < Nsamples; i++)
{
z = getVel();
Matrix *X = NULL;
X = intKalman(z);
xSaved->a[i][0] = time[i];
xSaved->a[i][1] = X->a[0][0];
xSaved->a[i][2] = X->a[1][0];
zSaved->a[i][0] = time[i];
zSaved->a[i][1] = z;
kill_M(&X);
}
const char fileX[] = "../data/X.dat";
const char fileZ[] = "../data/Z.dat";
MatrixPrint2File(xSaved, fileX);
MatrixPrint2File(zSaved, fileZ);
kill_M(&xSaved);
kill_M(&zSaved);
libera(time);
KalmanFilter_End();
EndProgram();
reportGood("O programa chegou ao fim!");
return(0);
}
float Brush::getAngle(){
if (tail.size() > 0){
ofPoint vel = getVel();
return atan2(vel.y, vel.x);
} else {
return 0.0f;
}
}
Real
PoisselleTubeBCValue::
value(const RealVect& a_point, const RealVect& a_normal, const Real& a_time, const int& a_comp) const
{
Real radius = this->getRadius(a_point);
RealVect velocity = getVel(radius);
return velocity[m_velComp];
}
bool MavlinkGpos::getGpos(GlobalPos* gpos) {
gpos->timestamp = mavlinkMessages_->global_position_int.time_boot_ms;
gpos->localTimestamp = mavlinkMessages_->lastGlobalPosition;
bool result = getPos(&gpos->position);
result &= getVel(&gpos->velocity);
return result;
}
bool Primitive::store(FILE* f) const {
const Pose& pose = getPose();
const Pos& vel = getVel();
const Pos& avel = getAngularVel();
if ( fwrite ( pose.ptr() , sizeof ( Pose::value_type), 16, f ) == 16 )
if( fwrite ( vel.ptr() , sizeof ( Pos::value_type), 3, f ) == 3 )
if( fwrite ( avel.ptr() , sizeof ( Pos::value_type), 3, f ) == 3 )
return true;
return false;
}
void Vehicle::print(bool full) {
if (full) {
std::cout << "--- vehicle ---" << std::endl;
std::cout << "carType:" << getTyp() << std::endl;
std::cout << "size: " << getWidth() << " x " << getLength() << std::endl;
std::cout << "maxAcc: " << getMaxAcc() << std::endl;
std::cout << "maxSpeed: " << getMaxSpeed() << std::endl;
std::cout << "LaneChangeTime: " << getLaneChangeTime() << std::endl;
std::cout << "Adress to lane: " << mpLane << std::endl;
}
std::cout << "pos: " << getPos() << " vel: " << getVel() << std::endl;
}
void Primitive::decellerate(double factorLin, double factorAng){
if(!body) return;
Pos vel;
if(factorLin!=0){
vel = getVel();
applyForce(vel*(-factorLin));
}
if(factorAng!=0){
vel = getAngularVel();
applyTorque(vel*(-factorAng));
}
}
AREXPORT void ArActionDesired::log(void) const
{
// all those maxes and movement parameters
if (getMaxVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tMaxTransVel %.0f", getMaxVel());
if (getMaxNegVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tMaxTransNegVel %.0f",
getMaxNegVel());
if (getTransAccelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tTransAccel %.0f", getTransAccel());
if (getTransDecelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tTransDecel %.0f", getTransDecel());
if (getMaxRotVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tMaxRotVel %.0f", "",
getMaxRotVel());
if (getRotAccelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tRotAccel %.0f", "",
getRotAccel());
if (getRotDecelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tRotDecel %.0f", "",
getRotDecel());
if (getMaxLeftLatVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tMaxLeftLatVel %.0f", "",
getMaxLeftLatVel());
if (getMaxRightLatVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tMaxRightLatVel %.0f", "",
getMaxRightLatVel());
if (getLatAccelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tLatAccel %.0f", "",
getLatAccel());
if (getLatDecelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tLatDecel %.0f", "",
getLatDecel());
// the actual movement part
if (getVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tVel %.0f", getVel());
if (getHeadingStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tHeading %.0f", "",
getHeading());
if (getDeltaHeadingStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tDeltaHeading %.0f", "",
getDeltaHeading());
if (getRotVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tRotVel %.0f", "",
getRotVel());
if (getLatVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tLatVel %.0f", "",
getLatVel());
}
void PlayerMelee::tick()
{
Game& core = Game::getInstance();
for (auto& p : core.level->getCollisions(getBB()))
{
if (p.first->isBreakable())
{
core.level->setTile(p.second.left/8.0+0.5,p.second.bottom/8.0+0.5, 0);
core.addEntity(new SmokeEffect(Coord(p.second.left+4.0,p.second.bottom+4.0)));
}
}
pos += getVel();
if (life>0) --life;
if (life == 0) dead = true;
}
void MobileEntity::placeAtEdge(const GameEntity& g)
{
///@todo implement
// collision detection for walls - so that the player can "land" on them and run into them
// perhaps abstract it to the Mobileentity level because other mobile entities will need hit detection for walls as well
CollisionBox b1=getHitBox();
CollisionBox b2=g.getHitBox();
//Tells whether or not the midpoint is at all within the mid point of the other box
vec3 d1=b1.getTranslate()-getVel()-b1.getDimensions()/3-b2.getPoint1();
vec3 d2=b1.getTranslate()-getVel()+b1.getDimensions()/3-b2.getPoint2();
vec3 midDist=b1.getTranslate()-b2.getTranslate();
bool midInX=(d1.x>0)!=(d2.x>0);
bool midInY=(d1.y>0)!=(d2.y>0);
bool midInZ=(d1.z>0)!=(d2.z>0);
if(!midInX&&!midInY&&getVel().z==0||
!midInX&&!midInZ&&getVel().y==0||
!midInZ&&!midInY&&getVel().x==0
)
return;
vec3 newPos;
//within the "x prism"
float dir=0;
float EASE_VAL=.05;
float EASE_SCALE_VAL=3.0;
if(midInY&&midInZ||!midInX){
if(midDist.x!=0)dir=midDist.x/abs(midDist.x);
else dir=0;
if(abs(d1.x)>b1.getDimensions().x/EASE_SCALE_VAL||abs(d2.x)>b1.getDimensions().x/EASE_SCALE_VAL)dir=0;
if(abs(d1.x)<abs(d2.x)){
if(getVel().x<0)
{
newPos.x=b2.getPoint1().x+b1.getDimensions().x/2;
}
}else{
if(getVel().x>0)
{
newPos.x=b2.getPoint2().x-b1.getDimensions().x/2;
}
}
if(newPos.x!=0){
setTranslateX(newPos.x);
setVelX(0);
}
translate(EASE_VAL*dir,0,0);
}
//within the "y prism"
if(midInX&&midInZ||!midInY){
if(midDist.y!=0)dir=midDist.y/abs(midDist.y);
else dir=0;
if(abs(d1.y)>b1.getDimensions().y/EASE_SCALE_VAL||abs(d2.y)>b1.getDimensions().y/EASE_SCALE_VAL)dir=0;
if(abs(d1.y)<abs(d2.y)){
if(getVel().y<0)
{
newPos.y=b2.getPoint1().y+b1.getDimensions().y/2;
_jumpCount=1;
}
}else{
if(getVel().y>0)
{
newPos.y=b2.getPoint2().y-b1.getDimensions().y/2;
}
}
if(newPos.y!=0){
setTranslateY(newPos.y);
setVelY(0);
}
setVel(getVel()*.85);
if(dot(getVel(),getVel())<.01){
setVel(0,0,0);
}
translate(0,EASE_VAL*dir,0);
}
//within the "z prism"
if(midInX&&midInY||!midInZ){
if(midDist.y!=0)dir=midDist.y/abs(midDist.y);
else dir=0;
if(abs(d1.z)>b1.getDimensions().z/EASE_SCALE_VAL||abs(d2.z)>b1.getDimensions().z/EASE_SCALE_VAL)dir=0;
if(abs(d1.z)<abs(d2.z)){
if(getVel().z<0)
{
newPos.z=b2.getPoint1().z+b1.getDimensions().z/2;
}
}else{
if(getVel().z>0)
{
newPos.z=b2.getPoint2().z-b1.getDimensions().z/2;
}
}
if(newPos.z!=0){
setTranslateZ(newPos.z);
setVelZ(0);
}
translate(0,0,EASE_VAL*dir);
}
vec3 v=getTranslate()-newPos;
}
void callback(const gazebo_msgs::ModelStates::ConstPtr& msg)
{
// ModelStates does not have a timestamp: work around: use own time!!
//throttle to about 20 Hz as Gazebo publishes at 1000Hz
ros::Time time_now = ros::Time::now();
static ros::Time time_last = time_now;
ros::Duration dur_last = time_now - time_last;
if (dur_last >= sleep_dur)
{
tf::Transform obj_t;
static tf::Transform obj_t_old;
tf::Transform rob;
float vel_now;
static float vel_old;
float acc_now;
static float acc_old;
float jerk_now;
visualization_msgs::MarkerArray marker_array;
std::vector<tf::Transform> poses = getObjectPose(msg, object_name);
rob = poses.at(0);
obj_t = poses.at(1);
//ROS_INFO("object origin: %4.2f,%4.2f",obj_t.getOrigin().getX(), obj_t.getOrigin().getY());
//Object frame and mesh
tf::StampedTransform tr(
//gaz2map_ * obj_t,
obj_t,
//rob.inverse() * obj_t * rob2try.inverse(),
time_now,
//std::string("/map"),
std::string("/gazebo_link"),
//std::string("/base_tray_link"),
std::string("/tray_object_1"));
br.sendTransform(tr);
marker_array.markers.push_back(makeMarker(tr, std::string("object_model")));
// Reference angle
tf::Quaternion ref_ang(0,0,0,1);
//ROS_INFO("Z axis: %4.2f, %4.2f, %4.2f",/*{{{*/
// tf::Matrix3x3(ref_ang).getColumn(2).getX(),
// tf::Matrix3x3(ref_ang).getColumn(2).getY(),
// tf::Matrix3x3(ref_ang).getColumn(2).getZ());/*}}}*/
ref_ang.setRPY(0,-M_PI/2,0);
tf::Transform obj_ref =
gaz2map_ * tf::Transform(ref_ang, obj_t.getOrigin());
marker_array.markers.push_back(makeMarker(tf::StampedTransform(
obj_ref,
time_now,
std::string("/map"),
std::string("/tray_object")), std::string("ref_angle"), true));
// actual object angle
tf::Transform obj_ang =
gaz2map_ * obj_t;// * tf::Transform(ref_ang, tf::Vector3(0,0,0));
tf::Vector3 z_ax_obj(
tf::Matrix3x3(obj_ang.getRotation()).getColumn(2).getX(),
tf::Matrix3x3(obj_ang.getRotation()).getColumn(2).getY(),
tf::Matrix3x3(obj_ang.getRotation()).getColumn(2).getZ());
//ROS_INFO("Z axis obj: %4.2f, %4.2f, %4.2f",/*{{{*/
// z_ax_obj.getX(),
// z_ax_obj.getY(),
// z_ax_obj.getZ());/*}}}*/
marker_array.markers.push_back(makeMarker(tf::StampedTransform(
obj_ang * tf::Transform(ref_ang, tf::Vector3(0,0,0)),
time_now,
std::string("/map"),
std::string("/tray_object")), std::string("obj_angle"), true));
// publish the marker
mark_pub->publish(marker_array);
//Angle between reference vector and object vector (z)
double ang =
acos(
z_ax_obj.getZ() / sqrt(
+z_ax_obj.getX()*z_ax_obj.getX()
+z_ax_obj.getY()*z_ax_obj.getY()
+z_ax_obj.getZ()*z_ax_obj.getZ()));
//ROS_INFO("Angle: %4.2f", ang);
geometry_msgs::Vector3Stamped dev_msg;
dev_msg.header.stamp = time_now;
dev_msg.header.frame_id = "/base_footprint";
dev_msg.vector.x = ang;
//relative translatory deviation object to tray
tf::Vector3 tray_obj = obj_t.getOrigin() - (rob*rob2obj).getOrigin();
//vector length is trans deviation
dev_msg.vector.y = sqrt(
+tray_obj.getX()*tray_obj.getX()
+tray_obj.getY()*tray_obj.getY()
+tray_obj.getZ()*tray_obj.getZ());
dev_pub->publish(dev_msg);
// object dynamcis
vel_now = getVel(obj_t.getOrigin(), obj_t_old.getOrigin(), dur_last);
vel_now = getMovingAverage(vel_now);
acc_now = getDerivation(vel_now, vel_old, dur_last);
jerk_now = getDerivation(acc_now, acc_old, dur_last);
float orient = getOrient(obj_t, obj_t_old);
//ROS_INFO("Obj. dynamics: %4.2f,%4.2f,%4.2f,%4.2f", vel_now, acc_now, jerk_now, orient);
std::vector<float> dyn = {vel_now, acc_now, jerk_now, orient};
publishDynamics(dyn, time_now);
obj_t_old = obj_t;
vel_old = vel_now;
acc_old = acc_now;
time_last = time_now;
}
}
void PhysicsAMotorJoint::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & WorldFieldMask)
{
if(_JointID)
{
dJointDestroy(_JointID);
_JointID = dJointCreateAMotor(getWorld()->getWorldID(), 0);
}
else
{
_JointID = dJointCreateAMotor(getWorld()->getWorldID(), 0);
if(!(whichField & VelFieldMask))
{
setVel(dJointGetAMotorParam(_JointID,dParamVel));
}
if(!(whichField & FMaxFieldMask))
{
setFMax(dJointGetAMotorParam(_JointID,dParamFMax));
}
if(!(whichField & FudgeFactorFieldMask))
{
setFudgeFactor(dJointGetAMotorParam(_JointID,dParamFudgeFactor));
}
if(!(whichField & Vel2FieldMask))
{
setVel2(dJointGetAMotorParam(_JointID,dParamVel2));
}
if(!(whichField & FMax2FieldMask))
{
setFMax2(dJointGetAMotorParam(_JointID,dParamFMax2));
}
if(!(whichField & FudgeFactor2FieldMask))
{
setFudgeFactor2(dJointGetAMotorParam(_JointID,dParamFudgeFactor2));
}
if(!(whichField & Vel3FieldMask))
{
setVel3(dJointGetAMotorParam(_JointID,dParamVel3));
}
if(!(whichField & FMax3FieldMask))
{
setFMax3(dJointGetAMotorParam(_JointID,dParamFMax3));
}
if(!(whichField & FudgeFactor3FieldMask))
{
setFudgeFactor3(dJointGetAMotorParam(_JointID,dParamFudgeFactor3));
}
if(!(whichField & HiStopFieldMask))
{
setHiStop(dJointGetAMotorParam(_JointID,dParamHiStop));
}
if(!(whichField & LoStopFieldMask))
{
setLoStop(dJointGetAMotorParam(_JointID,dParamLoStop));
}
if(!(whichField & BounceFieldMask))
{
setBounce(dJointGetAMotorParam(_JointID,dParamBounce));
}
if(!(whichField & CFMFieldMask))
{
setCFM(dJointGetAMotorParam(_JointID,dParamCFM));
}
if(!(whichField & StopCFMFieldMask))
{
setStopCFM(dJointGetAMotorParam(_JointID,dParamStopCFM));
}
if(!(whichField & StopERPFieldMask))
{
setStopERP(dJointGetAMotorParam(_JointID,dParamStopERP));
}
if(!(whichField & HiStop2FieldMask))
{
setHiStop2(dJointGetAMotorParam(_JointID,dParamHiStop2));
}
if(!(whichField & LoStop2FieldMask))
{
setLoStop2(dJointGetAMotorParam(_JointID,dParamLoStop2));
}
if(!(whichField & Bounce2FieldMask))
{
setBounce2(dJointGetAMotorParam(_JointID,dParamBounce2));
}
if(!(whichField & CFM2FieldMask))
{
setCFM2(dJointGetAMotorParam(_JointID,dParamCFM2));
}
if(!(whichField & StopCFM2FieldMask))
{
setStopCFM2(dJointGetAMotorParam(_JointID,dParamStopCFM2));
}
if(!(whichField & StopERP2FieldMask))
{
setStopERP2(dJointGetAMotorParam(_JointID,dParamStopERP2));
}
if(!(whichField & HiStop3FieldMask))
{
setHiStop3(dJointGetAMotorParam(_JointID,dParamHiStop3));
}
if(!(whichField & LoStop3FieldMask))
{
setLoStop3(dJointGetAMotorParam(_JointID,dParamLoStop3));
}
if(!(whichField & Bounce3FieldMask))
{
setBounce3(dJointGetAMotorParam(_JointID,dParamBounce3));
}
if(!(whichField & CFM3FieldMask))
{
setCFM3(dJointGetAMotorParam(_JointID,dParamCFM3));
}
if(!(whichField & StopCFM3FieldMask))
{
setStopCFM3(dJointGetAMotorParam(_JointID,dParamStopCFM3));
}
if(!(whichField & StopERP3FieldMask))
{
setStopERP3(dJointGetAMotorParam(_JointID,dParamStopERP3));
}
}
}
Inherited::changed(whichField, origin, details);
if((whichField & NumAxesFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetAMotorNumAxes(_JointID,getNumAxes());
}
if((whichField & Axis1FieldMask) ||
(whichField & Axis1ReferenceFrameFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetAMotorAxis(_JointID,0, getAxis1ReferenceFrame(), getAxis1().x(), getAxis1().y(), getAxis1().z());
}
if((whichField & Axis2FieldMask) ||
(whichField & Axis2ReferenceFrameFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetAMotorAxis(_JointID,1, getAxis2ReferenceFrame(), getAxis2().x(), getAxis2().y(), getAxis2().z());
}
if((whichField & Axis3FieldMask) ||
(whichField & Axis3ReferenceFrameFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetAMotorAxis(_JointID,2, getAxis3ReferenceFrame(), getAxis3().x(), getAxis3().y(), getAxis3().z());
}
if((whichField & VelFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamVel, getVel());
}
if((whichField & FMaxFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFMax, getFMax());
}
if((whichField & FudgeFactorFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFudgeFactor, getFudgeFactor());
}
if((whichField & Vel2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamVel2, getVel2());
}
if((whichField & FMax2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFMax2, getFMax2());
}
if((whichField & FudgeFactor2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFudgeFactor2, getFudgeFactor2());
}
if((whichField & Vel3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamVel3, getVel3());
}
if((whichField & FMax3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFMax3, getFMax3());
}
if((whichField & FudgeFactor3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFudgeFactor3, getFudgeFactor3());
}
if((whichField & HiStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamHiStop, getHiStop());
}
if((whichField & LoStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamLoStop, getLoStop());
}
if((whichField & BounceFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamBounce, getBounce());
}
if((whichField & CFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamCFM, getCFM());
}
if((whichField & StopERPFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopERP, getStopERP());
}
if((whichField & StopCFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopCFM, getStopCFM());
}
if((whichField & HiStop2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamHiStop2, getHiStop2());
}
if((whichField & LoStop2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamLoStop2, getLoStop2());
}
if((whichField & Bounce2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamBounce2, getBounce2());
}
if((whichField & CFM2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamCFM2, getCFM2());
}
if((whichField & StopERP2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopERP2, getStopERP2());
}
if((whichField & StopCFM2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopCFM2, getStopCFM2());
}
if((whichField & HiStop3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamHiStop3, getHiStop3());
}
if((whichField & LoStop3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamLoStop3, getLoStop3());
}
if((whichField & Bounce3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamBounce3, getBounce3());
}
if((whichField & CFM3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamCFM3, getCFM3());
}
if((whichField & StopERP3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopERP3, getStopERP3());
}
if((whichField & StopCFM3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopCFM3, getStopCFM3());
}
}
void Projectile::Move()
{
// Reset the acceleration each time in case we don't need to move.
setAcc(Vector2(0, 0));
if (attacking)
{
if (timerSet == false)
{
currentAttackTimer = attackTimer + sfw::getTime();
timerSet = true;
}
// Check what direction brings us towards the destination.
if (dest.x > getPos().x - 20)
{
if(getVel().x < -0.5f)
setAcc(Vector2(attackSpeed * 4, getAcc().y));
else
setAcc(Vector2(attackSpeed, getAcc().y));
}
else if (dest.x < getPos().x + 20)
{
if(getVel().x > 0.5f)
setAcc(Vector2(-attackSpeed * 4, getAcc().y));
else
setAcc(Vector2(-attackSpeed, getAcc().y));
}
if (dest.y > getPos().y - 20)
{
if(getVel().y < -0.5f)
setAcc(Vector2(getAcc().x, attackSpeed * 4));
else
setAcc(Vector2(getAcc().x, attackSpeed));
}
else if (dest.y < getPos().y + 20)
{
if(getVel().y > 0.5f)
setAcc(Vector2(getAcc().x, -attackSpeed * 4));
else
setAcc(Vector2(getAcc().x, -attackSpeed));
}
if (attackTimer < sfw::getTime())
{
attacking = false;
timerSet = false;
}
else if(getPos().x > dest.x - 5 && getPos().x < dest.x + 5 &&
getPos().y > dest.y - 5 && getPos().y < dest.y + 5)
{
attacking = false;
timerSet = false;
}
}
else
{
// Check what direction brings us towards the player.
if (game()->getPlayer().getPos().x >= getPos().x)
{
if (getVel().x < -0.5f)
setAcc(Vector2(accRate * 1.5f, getAcc().y));
else
setAcc(Vector2(accRate, getAcc().y));
}
else if (game()->getPlayer().getPos().x < getPos().x)
{
if (getVel().x > 0.5f)
setAcc(Vector2(-accRate * 1.5f, getAcc().y));
else
setAcc(Vector2(-accRate, getAcc().y));
}
if (game()->getPlayer().getPos().y >= getPos().y)
{
if (getVel().y < -0.5f)
setAcc(Vector2(getAcc().x, accRate * 1.5f));
else
setAcc(Vector2(getAcc().x, accRate));
}
else if (game()->getPlayer().getPos().y < getPos().y)
{
if (getVel().y > 0.5f)
setAcc(Vector2(getAcc().x, -accRate * 1.5f));
else
setAcc(Vector2(getAcc().x, -accRate));
}
}
// Apply the acceleration to the velocity
if (getAcc().x == 0 && getAcc().y == 0)
setVel(Vector2(getVel().x / 4, getVel().y / 4));
else if (getAcc().x == 0)
setVel(Vector2(getVel().x / 4, getVel().y + (getAcc().y * sfw::getDeltaTime())));
else if (getAcc().y == 0)
setVel(Vector2(getVel().x + (getAcc().x * sfw::getDeltaTime()), getVel().y / 4));
else
setVel(Vector2(getVel().x + (getAcc().x * sfw::getDeltaTime()), getVel().y + (getAcc().y * sfw::getDeltaTime())));
applyMaxSpeed();
// Use all the information we just calculated to move accordingly
setPos(Vector2(getPos().x + getVel().x, getPos().y + getVel().y));
}
void line_width() {
TCanvas *c1 = MakeCanvas("c1", "c1", 800, 600);
TF1 *fitPeak = new TF1("fitPeak", "[0]-[1]*([3]/2)^2/((x-[2])^2+([3]/2)^2)-[4]*([6]/2)^2/((x-[5])^2+([6]/2)^2)-[7]*([9]/2)^2/((x-[8])^2+([9]/2)^2)-[10]*([12]/2)^2/((x-[11])^2+([12]/2)^2)-[13]*([15]/2)^2/((x-[14])^2+([15]/2)^2)-[16]*([18]/2)^2/((x-[17])^2+([18]/2)^2)-[19]*(x-[20])**2",200, 2040);
char outfile[50];
vector<Double_t> linewidths;
vector<Double_t> linewidthsunc;
vector<Double_t> err1;
vector<Double_t> err2;
for (Int_t i=21; i<27; i++) {
TH1D *fecn = new TH1D("fecn", "fecn", 2048, 0, 2048);
TH1D *cal = new TH1D("cal", "cal", 2048, 0, 2048);
confParse("data_list.txt", i, fecn);
if ((i==21)||(i==26)) confParse("data_list.txt", 4, cal);
else confParse("data_list.txt", 30, cal);
fitPeak->SetParameter(0, 120);
fitPeak->SetParameter(1, 20);
fitPeak->SetParameter(2, 500);
fitPeak->SetParameter(3, 15);
fitPeak->SetParameter(4, 20);
fitPeak->SetParameter(5, 700);
fitPeak->SetParameter(6, 15);
fitPeak->SetParameter(7, 200);
fitPeak->SetParameter(8, 1000);
fitPeak->SetParameter(9, 15);
fitPeak->SetParameter(10, 20);
fitPeak->SetParameter(11, 1100);
fitPeak->SetParameter(12, 15);
fitPeak->SetParameter(13, 20);
fitPeak->SetParameter(14, 1400);
fitPeak->SetParameter(15, 15);
fitPeak->SetParameter(16, 20);
fitPeak->SetParameter(17, 1600);
fitPeak->SetParameter(18, 15);
cal->SetTitle("");
cal->GetXaxis()->SetTitle("MCA Channel");
cal->GetYaxis()->SetTitle("Counts");
cal->GetXaxis()->SetNdivisions(8,5,0);
cal->SetLineColor(kRed);
cal->Fit("fitPeak", "R");
sprintf(outfile, "linewidthcal%i.png", i);
c1->SaveAs(outfile);
vector<Double_t> peakPos;
vector<Double_t> peakPosUnc;
peakPos.push_back(fitPeak->GetParameter(2)); peakPosUnc.push_back(TMath::Sqrt(fabs(fitPeak->GetParameter(3))));
peakPos.push_back(fitPeak->GetParameter(5)); peakPosUnc.push_back(TMath::Sqrt(fabs(fitPeak->GetParameter(6))));
peakPos.push_back(fitPeak->GetParameter(8)); peakPosUnc.push_back(TMath::Sqrt(fabs(fitPeak->GetParameter(9))));
peakPos.push_back(fitPeak->GetParameter(11)); peakPosUnc.push_back(TMath::Sqrt(fabs(fitPeak->GetParameter(12))));
peakPos.push_back(fitPeak->GetParameter(14)); peakPosUnc.push_back(TMath::Sqrt(fabs(fitPeak->GetParameter(15))));
peakPos.push_back(fitPeak->GetParameter(17)); peakPosUnc.push_back(TMath::Sqrt(fabs(fitPeak->GetParameter(18))));
TF1 *velCurve = new TF1("velCurve", "[0]*x+[1]*x^2+[2]", 200, 2040);
getVel(peakPos, peakPosUnc, velCurve);
//TF1 *fitPeak2 = new TF1("fitPeak2", "[0]-[1]*([3]/2)^2/((x-[2])^2+([3]/2)^2)", 800, 1300);
TF1 *fitPeak2 = new TF1("fitPeak2", "[0]-[1]*([3]/2)^2/((x-[2])^2+([3]/2)^2)", 900, 1200);
//TF1 *fitPeak2 = new TF1("fitPeak2", "[0]-[1]*([3]/2)^2/((x-[2])^2+([3]/2)^2)-[4]*(x-[5])", 200, 2000);
fitPeak2->SetParameter(0, 21500);
fitPeak2->SetParameter(1, 2000);
fitPeak2->SetParameter(2, 1040);
fitPeak2->SetParameter(3, 50);
fecn->SetTitle("");
fecn->GetXaxis()->SetRangeUser(800, 1300);
fecn->GetXaxis()->SetTitle("MCA Channel");
fecn->GetYaxis()->SetTitle("Counts");
fecn->GetXaxis()->SetNdivisions(8,5,0);
fecn->SetLineColor(kRed);
fecn->Draw();
fecn->Fit("fitPeak2", "RN");
fitPeak2->Draw("same");
sprintf(outfile, "linewidth%i.png", i);
c1->SaveAs(outfile);
Double_t blah = fabs(velCurve->Eval(fitPeak2->GetParameter(2)+fitPeak2->GetParameter(3)/2)-velCurve->Eval(fitPeak2->GetParameter(2)-fitPeak2->GetParameter(3)/2));
linewidths.push_back(fabs(velCurve->Eval(fitPeak2->GetParameter(2)+fitPeak2->GetParameter(3)/2)-velCurve->Eval(fitPeak2->GetParameter(2)-fitPeak2->GetParameter(3)/2)));
Double_t temp1=fabs(velCurve->Eval(fitPeak2->GetParameter(2)+fitPeak2->GetParameter(3)/2-fitPeak2->GetParError(3)/2)-velCurve->Eval(fitPeak2->GetParameter(2)-fitPeak2->GetParameter(3)/2+fitPeak2->GetParError(3)/2));
Double_t temp2=fabs(velCurve->Eval(fitPeak2->GetParameter(2)+fitPeak2->GetParameter(3)/2+fitPeak2->GetParError(3)/2)-velCurve->Eval(fitPeak2->GetParameter(2)-fitPeak2->GetParameter(3)/2-fitPeak2->GetParError(3)/2));
Double_t moreblah = TMath::Max(fabs(temp1-blah), fabs(temp2-blah));
Double_t andagain = fabs(getVelUnc(velCurve->Eval(fitPeak2->GetParameter(2)+fitPeak2->GetParameter(3)/2)));
//Double_t andagain=0;
err1.push_back(moreblah);
err2.push_back(andagain);
linewidthsunc.push_back(TMath::Sqrt(moreblah*moreblah+andagain*andagain));
delete cal;
delete fecn;
peakPos.clear();
peakPosUnc.clear();
}
TGraphErrors *gr = new TGraphErrors();
for (Int_t i=0; i<linewidths.size(); i++) {
cout << linewidths[i] << "\\pm" << err1[i] << "\\pm" << err2[i] << "\\pm" << linewidthsunc[i] << endl;
gr->SetPoint(i, 25*(i+1), linewidths[i]);
gr->SetPointError(i,0, linewidthsunc[i]);
}
gr->SetTitle("");
gr->GetXaxis()->SetTitle("Absorber Thickness [mg/cm^{2}]");
gr->GetYaxis()->SetTitle("Line Width [mm/s]");
gr->Draw("ap");
TF1 *thing = new TF1("thing", "[0]+[1]*x",0,7);
gr->Fit("thing");
Double_t nomE=14.4e3;
Double_t cLight=3e11;
cout << thing->GetParameter(0)*nomE/cLight << " " << thing->GetParError(0)*nomE/cLight << endl;
c1->SaveAs("linewidth.png");
}
