void ftDrawForce::update() {
switch (drawType.get()) {
case 1:
if (type != FT_DENSITY) {
saveValue(type, force);
setForce(density);
reset();
}
setType(FT_DENSITY);
break;
case 2:
if (type != FT_VELOCITY) {
saveValue(type, force);
setForce(velocity);
reset();
}
force.set(ofVec4f(force.get().x, force.get().y, 0, 1));
setType(FT_VELOCITY);
break;
case 3:
if (type != FT_TEMPERATURE) {
saveValue(type, force);
setForce(temperature);
reset();
}
force.set(ofVec4f(force.get().x, 0, 0, 1));
setType(FT_TEMPERATURE);
break;
case 4:
if (type != FT_PRESSURE) {
saveValue(type, force);
setForce(pressure);
reset();
}
force.set(ofVec4f(force.get().x, 0, 0, 1));
setType(FT_PRESSURE);
break;
case 5:
if (type != FT_OBSTACLE) {
saveValue(type, force);
setForce(obstacle);
reset();
}
force.set(ofVec4f((int)abs(force.get().x + 0.5), 0, 0, 1));
setType(FT_OBSTACLE);
break;
default:
setType(FT_NONE);
break;
}
if(doReset) {
reset();
doReset.set(false);
}
if (forceApplied) forceChanged = true;
else if (isTemporary) forceChanged = false;
forceApplied = false;
}
void
Contact::output(const TaskInfo& taskInfo)
{
if (nonZeroIntersection(taskInfo.getSampleTimeSet(),
SampleTime::PerTimestep)) {
Log(Model, Debug) << "Contact::output(): \"" << getName()
<< "\" computing ground plane below" << endl;
getGround(taskInfo.getTime());
}
// Transform the plane equation to the local frame.
Plane lp = mMountFrame->planeFromRef(mGroundVal.plane);
// Get the intersection length.
real_type compressLength = lp.getDist(Vector3::zeros());
// Don't bother if we do not intersect the ground.
if (compressLength < 0) {
setForce(Vector6::zeros());
return;
}
// The velocity of the ground patch in the current frame.
Vector6 groundVel(mMountFrame->rotFromRef(mGroundVal.vel.getAngular()),
mMountFrame->rotFromRef(mGroundVal.vel.getLinear()));
groundVel -= mMountFrame->getRefVel();
// Now get the relative velocity of the ground wrt the contact point
Vector3 relVel = - groundVel.getLinear();
// The velocity perpandicular to the plane.
// Positive when the contact spring is compressed,
// negative when decompressed.
real_type compressVel = - lp.scalarProjectToNormal(relVel);
// The in plane velocity.
Vector3 sVel = lp.projectToPlane(relVel);
// Get the plane normal force.
real_type normForce = computeNormalForce(compressLength, compressVel);
// The normal force cannot get negative here.
normForce = max(static_cast<real_type>(0), normForce);
// Get the friction force.
Vector3 fricForce = computeFrictionForce(normForce, sVel, lp.getNormal(),
mGroundVal.friction);
// The resulting force is the sum of both.
// The minus sign is because of the direction of the surface normal.
Vector3 force = fricForce - normForce*lp.getNormal();
// We don't have an angular moment.
setForce(Vector6(Vector3::zeros(), force));
}
// update〜系関数をまとめる
void update(float bar_move) {
// もし失敗していたら
if(miss) {
// 落下させる
setForce(0, 0.3);
}
// 失敗していない場合は
else {
// バーの動きと逆方向に力がかかる（すべるような動き）
setForce(-bar_move, 0);
updateForce();
}
updateVelocity();
updatePos();
}
void lowerDown(int force)
{
setForce(force);
AT91F_PIO_SetOutput( AT91C_BASE_PIOA, DIRD);
AT91F_PIO_ClearOutput( AT91C_BASE_PIOA, DIRC);
direction = 1;
}
bool CannonField::qt_invoke( int _id, QUObject* _o )
{
switch ( _id - staticMetaObject()->slotOffset() ) {
case 0:
setAngle((int)static_QUType_int.get(_o+1));
break;
case 1:
setForce((int)static_QUType_int.get(_o+1));
break;
case 2:
shoot();
break;
case 3:
newTarget();
break;
case 4:
setGameOver();
break;
case 5:
restartGame();
break;
case 6:
moveShot();
break;
default:
return QWidget::qt_invoke( _id, _o );
}
return TRUE;
}
//===========================================================================
cPhantom::~cPhantom()
{
// SET ZERO FORCE:
setForce(0,0,0);
// UNMAP FILE:
UnmapViewOfFile(lpMapAddress);
}
void liftUp(int force)
{
setForce(force);
AT91F_PIO_SetOutput( AT91C_BASE_PIOA, DIRC);
AT91F_PIO_ClearOutput( AT91C_BASE_PIOA, DIRD);
direction = -1;
}
bool CannonField::qt_invoke( int _id, QUObject* _o )
{
switch ( _id - staticMetaObject()->slotOffset() ) {
case 0: setAngle((int)static_QUType_int.get(_o+1)); break;
case 1: setForce((int)static_QUType_int.get(_o+1)); break;
default:
return QWidget::qt_invoke( _id, _o );
}
return TRUE;
}
void CScreamer::sweepHitConsequences(const std::vector<Physics::CSweepHit> &hits){
for(auto it = hits.begin(); it < hits.end(); ++it){
if((*it).entity->getName() != _entity->getName()){
Vector3 direct = -(_directionShoot.reflect(-(*it).normal));
auto m = std::make_shared<CMessageAddForcePlayer>();
Vector3 temp = (_directionShoot + (Vector3(0.0f,1.0f,0.0f))).normalisedCopy();
m->setForce(temp * (_screamerScreamForce*(1.0f- (*it).distance/_screamerScreamMaxDistance)));
(*it).entity->emitMessage(m);
}
}
} // sweepHitConsequences
Emitter::Emitter(const Ref<Zone>& zone,bool full,int tank,float flow,float forceMin,float forceMax) :
Transformable(),
active(true),
full(full),
zone(!zone ? SPK_DEFAULT_ZONE : zone),
flow(1.0f),
fraction(SPK_RANDOM(0.0f,1.0f))
{
setTank(tank);
setFlow(flow);
setForce(forceMin,forceMax);
}
void
Engine::setEngine(unsigned int engid, double force[3], double pos[3], double orient[3], float max_force)
{
m_act = 0;
m_id = encodeId(engid);
setForce(force[0], force[1], force[2], false, false);
setPosition(pos[0], pos[1], pos[2], false);
m_act_orientation[0] = orient[0];
m_act_orientation[1] = orient[1];
m_act_orientation[2] = orient[2];
m_engine_max_force = max_force;
}
Thug* Thug::createMonster(std::vector<Point> points)
{
auto monster = new Thug();
if (monster && monster->init())
{
monster->setPointsVector(points);
monster->setMaxHp(35);
monster->setCurrHp(35);
monster->setMoney(10);
monster->setForce(4);
monster->setArmor(0);
monster->setForce(8);
monster->setAttackBySoldier(true);
monster->setRunSpeed(40);
monster->runNextPoint();
monster->autorelease();
return monster;
}
CC_SAFE_DELETE(monster);
return NULL;
}
void HelloWorld::setPlayerForce(float nhp)
{
auto s = (Submarine*)playerSub;
auto n = (Notifier*)notifier;
s->setForce(nhp);
float displayForce = getPlayerForce();
if(playerTurning)
displayForce += s->getTurningForce();
float displayRatio = displayForce / getPlayerMaxForce();
n->setThrustBar(displayRatio);
n->setThrustText(displayRatio * 100);
float noise = s->getNoiseLevel(displayRatio);
n->setNoiseBar(noise);
n->setNoiseText(noise);
}
ForceMeter::ForceMeter(int posX, int posY, int sizeX, int sizeY){
sf::Vector2f newSize(sizeX, sizeY);
m_size = newSize;
m_body.setSize(m_size);
m_body.setPosition(posX, posY);
m_body.setFillColor(sf::Color::White);
meterBody.setPosition(posX + sizeX/2, posY);
meterBody.setSize(sf::Vector2f(sizeX/100.0, sizeY));
meterBody.setFillColor(sf::Color::Blue);
setForce(0);
update();
}
//===========================================================================
int cHydraDevice::setForceAndTorque(const cVector3d& a_force,
const cVector3d& a_torque)
{
/************************************************************************
STEP 10:
Here you may implement code which sends a torque command (Tx,Ty,Tz)
to your haptic device. This would be implemented if you have
a haptic device with an active stylus for instance.
If the operation fails return an error code such as -1 for instance.
If your device does not support torque capabilities, you can simply
ignore this function. This method will simply ignore the torque component
and simply send the force component to your device.
Note that for optimization purposes, you may want to replace the
following line:
error = setForce(a_force);
with code that directly applies force and torque components to your
device.
Note:
For consistency, units must be in Newton meters for torques.
A torque (1,0,0) would rotate counter clock-wise around the x-axis.
*************************************************************************/
int error = 0;
// send force to the haptic device (see instructions above)
error = setForce(a_force);
// store new torque values
m_prevTorque = a_torque;
// *** INSERT YOUR CODE HERE ***
// double tx = a_torque(0) ;
// double ty = a_torque(1) ;
// double tz = a_torque(2) ;
// setTorqueToMyDevice(tx, ty, tz);
// exit
return (error);
}
ReinforceSoldier2* ReinforceSoldier2::createReinforceSoldier(Point point)
{
auto reinforceSoldier = new ReinforceSoldier2();
if (reinforceSoldier && reinforceSoldier->init())
{
reinforceSoldier->setLocation(point);
reinforceSoldier->setMaxHp(80);
reinforceSoldier->setCurrHp(80);
reinforceSoldier->setForce(10);
reinforceSoldier->setState(SoldierStateNone);
reinforceSoldier->birth(point);
reinforceSoldier->autorelease();
return reinforceSoldier;
}
CC_SAFE_DELETE(reinforceSoldier);
return NULL;
}
void Wheel::processReadyRead()
{
if (serial->bytesAvailable() >= sizeof(wheel_report_t)+2) {
if (serial->read(1).at(0) == (char)0xAA) {
if (serial->read(1).at(0) == (char)0xAA) {
char inBuffer[sizeof(wheel_report_t)+1];
serial->read(inBuffer, sizeof(wheel_report_t));
wheel_report_t wReport;
memcpy(&wReport, inBuffer, sizeof(wheel_report_t));
vjoy->updateWheelStatus(&wReport);
}
}
if (serial->bytesAvailable() > (sizeof(wheel_report_t)+2)*2)
serial->readAll();
}
setForce(vjoy->force);
}
void RCPBody::create()
{
if (RCPWorld::instance()) {
cpSpace * space = RCPWorld::instance()->space();
if (mType == Dynamic) {
mBody = cpSpaceAddBody(space, cpBodyNew(mMass, mMoment));
} else if (mType == Static) {
mBody = cpSpaceGetStaticBody(space);
}
}
setForce(mForce);
setVelocity(mVelocity);
setPosition(mPosition);
setAngle(mAngle);
setUserData(mUserData);
}
void setPosition(int force)
{
setForce(force);
if (abs(curPos - position) > 5)
{
if (curPos > position)
{
lowerDown(force);
}
if (curPos < position)
{
liftUp(force);
}
}
else
{
releasePosition();
}
}
Gorilla* Gorilla::createMonster(std::vector<Point> points)
{
auto monster = new Gorilla();
if (monster && monster->init())
{
monster->setPointsVector(points);
monster->setMaxHp(2500);
monster->setCurrHp(2500);
monster->setMoney(500);
monster->setForce(60);
monster->setArmor(0);
monster->setAttackBySoldier(true);
monster->setRunSpeed(30);
monster->runNextPoint();
monster->autorelease();
return monster;
}
CC_SAFE_DELETE(monster);
return NULL;
}
Quetzal* Quetzal::createMonster(std::vector<Point> points)
{
auto monster = new Quetzal();
if (monster && monster->init())
{
monster->setPointsVector(points);
monster->setMaxHp(500);
monster->setCurrHp(500);
monster->setMoney(20);
monster->setForce(45);
monster->setArmor(10);
monster->setAttackBySoldier(false);
monster->setRunSpeed(60);
monster->runNextPoint();
monster->autorelease();
return monster;
}
CC_SAFE_DELETE(monster);
return NULL;
}
void CScreamer::raycastHitConsequences(const Physics::CRaycastHit &hitWorld){
if(hitWorld.distance < _screamerReboundDistance){
auto m = std::make_shared<CMessageAddForcePlayer>();
m->setForce(-_directionShoot * (_screamerReboundForce*( 1.0f - hitWorld.distance/_screamerScreamMaxDistance)));
_entity->emitMessage(m);
_rebound = 0;
}else{
if(_rebound < _screamerMaxRebounds){
++_rebound;
_positionShoot = hitWorld.impact;
// los dos reflejos que quedan bien son
// _directionShoot.reflect(hitWorld.normal)
// (_directionShoot + hitWorld.normal).normalisedCopy()
_directionShoot = _directionShoot.reflect(hitWorld.normal);
_distanceShoot -= hitWorld.distance;
primarySkill();
}else{
_rebound = 0;
}
}
} // raycastHitConsequences
void CPHCharacter::set_State(const SPHNetState& state)
{
m_body_interpolation.SetPosition(state.previous_position,0);
m_body_interpolation.SetPosition(state.position,1);
SetPosition(state.position);
SetVelocity(state.linear_vel);
setForce(state.force);
// SetAcceleration(state.accel);
// SetMaximumVelocity(state.max_velocity);
if(!b_exist) return;
if(state.enabled)
{
Enable();
};
if(!state.enabled )
{
Disable();
};
VERIFY2(dBodyStateValide(m_body),"WRONG BODYSTATE WAS SET");
}
Boss_Efreeti* Boss_Efreeti::createMonster(std::vector<Point> points,std::vector<std::vector<std::vector<Point>>> sonpath)
{
auto monster = new Boss_Efreeti();
if (monster && monster->init())
{
monster->setPointsVector(points);
monster->setMaxHp(5550);
monster->setCurrHp(5550);
monster->setMoney(1500);
monster->sonPath = sonpath;
monster->setForce(50);
monster->setArmor(10);
monster->setAttackByTower(true);
monster->setAttackBySoldier(true);
monster->setRunSpeed(20);
monster->runNextPoint();
monster->autorelease();
return monster;
}
CC_SAFE_DELETE(monster);
return NULL;
}
void PhysShape::unpack(BitStream* stream)
{
bool enabled = stream->readFlag();
if (isEnabled()!=enabled)
setEnable(enabled);
if (enabled)
{
bool active = stream->readFlag();
bool toInactive = false;
if (!active)
{
toInactive = stream->readFlag();
if (!toInactive)
return;
}
if (!isActive())
setActive(true);
QuatF q;
VectorF vec;
mathRead(*stream, &vec);
setPosition(vec);
mathRead(*stream, &q);
setRotation(q);
mathRead(*stream, &vec);
setForce(vec);
mathRead(*stream, &vec);
setTorque(vec);
mathRead(*stream, &vec);
setLinVelocity(vec);
mathRead(*stream, &vec);
setAngVelocity(vec);
if (toInactive)
setActive(false);
}
}
//===========================================================================
int cMyCustomDevice::setForceAndTorqueAndGripper(cVector3d& a_force, cVector3d& a_torque, double a_gripperTorque)
{
// temp variables
int error = 0;
int result = 0;
/************************************************************************
STEP 11:
Here you may implement code which send a force, torque and gripper
command at once. For some haptic devices, this is a more optimal
way of proceeding which limits the amount of data being transfered
over to the device.
You may ignore this section in which case the individuals methods
are called consecutively.
*************************************************************************/
// *** INSERT YOUR CODE HERE, MODIFY CODE BELLOW ACCORDINGLY ***
// send a force command
m_prevForce = a_force;
error = setForce(a_force);
if (error != 0) { result = error; }
// send a torque command
m_prevTorque = a_torque;
error = setTorque(a_torque);
if (error != 0) { result = error; }
// send a gripper torque command
m_prevGripperTorque = a_gripperTorque;
error = setGripperTorque(a_gripperTorque);
if (error != 0) { result = error; }
// exit
return (result);
}
void Node::reset()
{
particles_near_node = 0;
setForce(0.0f, 0.0f, 0.0f);
}
//*--------------------------------------------------------------------------------------
//* Function Name : Main
//* Object : Software entry point
//* Input Parameters : none.
//* Output Parameters : none.
//*--------------------------------------------------------------------------------------
int main(void)
{
char data[MSG_SIZE];
unsigned int length;
int stepCnt = 0;
unsigned char str[10];
/**** System init ****/
//InitFrec();
Init_CP_WP();
//chek for CP and WP
//CP - card present
while(((AT91C_BASE_PIOA->PIO_PDSR) & BIT15)) { /*put your card present event here*/ }
//WP - write protect
//while(((AT91C_BASE_PIOA->PIO_PDSR) & BIT16)) { /*put your write protect event here*/ }
if (initMMC() == MMC_SUCCESS)
{
//card_state |= 1;
memset(&mmc_buffer,0,512);
mmcReadRegister (10, 16);
mmc_buffer[7]=0;
}
flashInit();
Init_PWM();
// Enable User Reset and set its minimal assertion to 960 us
AT91C_BASE_RSTC->RSTC_RMR = AT91C_RSTC_URSTEN | (0x4<<8) | (unsigned int)(0xA5<<24);
// Led init
// First, enable the clock of the PIOB
AT91F_PMC_EnablePeriphClock ( AT91C_BASE_PMC, 1 << AT91C_ID_PIOA ) ;
//* to be outputs. No need to set these pins to be driven by the PIO because it is GPIO pins only.
AT91F_PIO_CfgOutput( AT91C_BASE_PIOA, OUTPUT_MASK );
//* Clear the LED's.
/*
AT91F_PIO_SetOutput( AT91C_BASE_PIOA, OUTPUT_MASK );
AT91F_PIO_ClearOutput( AT91C_BASE_PIOA, OUTPUT_MASK );
*/
// Init USB device
AT91F_USB_Open();
AT91F_PIO_ClearOutput( AT91C_BASE_PIOA, OUTPUT_MASK );
// Init USB device
// Wait for the end of enumeration
setForce(40000);
int pCDCEnablingCounter = 0;
while (!pCDC.IsConfigured(&pCDC) && pCDCEnablingCounter < 2500000){ pCDCEnablingCounter++; };
if (pCDCEnablingCounter < 2500000)
{
CDC = 1;
}
setForce(0);
// Set Usart in interrupt
//Usart_init();
//Read and set settings
memcpy(settings, OUR_FLASH_ADDR, 128);
int i;memset(&mmc_buffer, 0x00, 512);
int j;
char *settingsBlocks[50];
char settingsDelim[] = "~";
char *settingsParts = strtok( settings, settingsDelim );
i = 0;
while( settingsParts != NULL )
{
settingsBlocks[i++] = settingsParts;
settingsParts = strtok( NULL, settingsDelim );
}
for (j = 0; j < i; j++)
{
parseSettings(settingsBlocks[j]);
}
InitADC();
Init_PWM();
AT91F_PIO_CfgInput(AT91C_BASE_PIOA, SW1_MASK);
AT91F_PIO_CfgInput(AT91C_BASE_PIOA, SW2_MASK);
AT91F_PIO_SetOutput( AT91C_BASE_PIOA, LED_GREEN);
AT91F_PIO_SetOutput( AT91C_BASE_PIOA, LED_YELLOW);
setForce(0);
//startBlinking(250000);
/**** MMC CARD ****/
init_extint();
while (1)
{
cnt++;
if (cnt > 50000)
{
cnt = 0;
printTrace("COUNTER RESET\n");
}
}
}
extern void shapeop_editVertexForce(ShapeOpSolver *op, int force_id, ShapeOpScalar *force, int id) {
Eigen::Map<ShapeOp::Vector3> g(force, 3, 1);
auto f = std::dynamic_pointer_cast<ShapeOp::VertexForce>(op->s->getForce(force_id)); //TODO: this will need to be robustify
f->setId(id);
f->setForce(g);
}
//==============================================================================
void DegreeOfFreedom::resetForce()
{
setForce(0.0);
}
