void interpolateZXY(){
// if(interpolationCounter<5){
// interpolationCounter++;
// return;
// }
interpolationCounter=0;
if(!configured){
HomeLinks();
return;
}
keepCartesianPosition=TRUE;
if(keepCartesianPosition){
float x=0,y=0,z=0;
float ms= getMs();
x = interpolate((INTERPOLATE_DATA *)&intCartesian[0],ms);
y = interpolate((INTERPOLATE_DATA *)&intCartesian[1],ms);
z = interpolate((INTERPOLATE_DATA *)&intCartesian[2],ms);
if(isCartesianInterpolationDone() == FALSE){
// println_W("Interp \r\n\tx=");p_fl_W(x);print_W(" \tc=");p_fl_W(xCurrent);
//
// print_W("\r\n\ty=");p_fl_W(y);print_W(" \tc=");p_fl_W(yCurrent);
//
// print_W("\r\n\tz=");p_fl_W(z);print_W(" \tc=");p_fl_W(zCurrent);
setXYZ( x, y, z, 0);
}else if( FifoGetPacketCount(&packetFifo)>0){
println_W("Loading new packet ");
if(FifoGetPacket(&packetFifo,&linTmpPack)){
processLinearInterpPacket(&linTmpPack);
}
}else{
keepCartesianPosition=FALSE;
}
}
}
BYTE setInterpolateXYZ(float x, float y, float z,float ms){
int i=0;
if(ms<.01)
ms=0;
float start = getMs();
intCartesian[0].set=x;
intCartesian[1].set=y;
intCartesian[2].set=z;
updateCurrentPositions();
intCartesian[0].start=xCurrent;
intCartesian[1].start=yCurrent;
intCartesian[2].start=zCurrent;
println_W("\n\nSetting new position x=");p_fl_W(x);print_W(" y=");p_fl_W(y);print_W(" z=");p_fl_W(z);print_W(" Time MS=");p_fl_W(ms);
println_W("Current position cx=");p_fl_W(xCurrent);
print_W(" cy=");p_fl_W(yCurrent);
print_W(" cz=");p_fl_W(zCurrent);
println_W("Current angles Alpha=");p_fl_W(getLinkAngle(0));print_W(" Beta=");p_fl_W(getLinkAngle(1));print_W(" Gamma=");p_fl_W(getLinkAngle(2));
for(i=0;i<3;i++){
intCartesian[i].setTime=ms;
intCartesian[i].startTime=start;
}
if(ms==0){
setXYZ( x, y, z,0);
}else{
keepCartesianPosition=TRUE;
float ms= getMs();
x = interpolate((INTERPOLATE_DATA *)&intCartesian[0],ms);
y = interpolate((INTERPOLATE_DATA *)&intCartesian[1],ms);
z = interpolate((INTERPOLATE_DATA *)&intCartesian[2],ms);
setXYZ( x, y, z,0);
}
}
/**
* Processes the properties of the body, shape, mass friction, etc...
*/
void RigidBody::processBody_() {
// Remove the current body from the physics engine
selfRemoveBody_();
if(!shape_) {
return;
}
shape_->calculateLocalInertia(mass_, inertia_);
float x = getX();
float y = getY();
float z = getZ();
if(body_) {
delete(body_);
}// else {
//btTransform xform;
//xform = motionState_.getWorldTransform();
//xform.setRotation(btQuaternion (btVector3(getRotX(), getRotY(), getRotZ()), getRotAngle()*(PI/180)));
//xform.setOrigin(
//((btPairCachingGhostObject*)body_)->setWorldTransform (xform);
//motionState_->setWorldTransform (xform);
btTransform xform;
motionState_->getWorldTransform(xform);
xform.setOrigin(btVector3(getX(), getY(), getZ()));
motionState_->setWorldTransform(xform);
//}
btRigidBody::btRigidBodyConstructionInfo rigidBodyCI(
mass_,
motionState_,
shape_,
inertia_
);
rigidBodyCI.m_friction = friction_;
rigidBodyCI.m_mass = mass_;
body_ = new btRigidBody(rigidBodyCI);
setXYZ(x, y, z);
selfAddBody_();
/*btRigidBody* rb = dynamic_cast<btRigidBody*>(body_);
if(rb) {
rb->clearForces();
}*/
stopMovement();
}
/**
* Homothétie sur le mesh, ayant pour centre le centre de gravité du
* mesh.
* @param x rapport de l'homothétie.
*/
void Mesh::homothetie(float x)
{
QMatrix4x4 t;
t.translate(centre_);
t.scale(x);
t.translate(-centre_);
for (int i=0;i<nbVertices_;i++)
{
QVector3D &v=vertices_[i];
setXYZ(i, (t*v));
}
octree_->changerTailleOctree(x-1);
}
void Vector::norm()
{
formulate();
double t=length();
//cout <<" $ ";
if (fabs(t)<ZERO){
cout<<"\nERROR:Vector::norm() failed,t="<<t<<endl;
cout<<"\n (XYZ)=("<< vm[0]<<","<<vm[1]<<","<<vm[2]<<")"<<endl;
setXYZ(0.0f,0.0f,0.0f);
}else{
vm[0] /= t;
vm[1] /= t;
vm[2] /= t;
}
}
int Vector::normJudge()
{
formulate();
double t=length();
if (fabs(t)<ZERO){
//printf("\nERROR:Vector::norm() failed");
setXYZ(0.0f,0.0f,0.0f);
return -1;
}else{
vm[0] /= t;
vm[1] /= t;
vm[2] /= t;
return 1;
}
}
void Item::setup(int itemId, int setId, int animationId, Vector3 position, int facing, int height, int width, bool isTargetFlag, bool isVisibleFlag, bool isPoliceMazeEnemyFlag) {
_itemId = itemId;
_setId = setId;
_animationId = animationId;
_facing = facing;
_angle = facing * (M_PI / 512.0f);
_width = width;
_height = height;
_isTarget = isTargetFlag;
_isVisible = isVisibleFlag;
_isPoliceMazeEnemy = isPoliceMazeEnemyFlag;
setXYZ(position);
_screenRectangle.bottom = -1;
_screenRectangle.right = -1;
_screenRectangle.top = -1;
_screenRectangle.left = -1;
}
/** Checks if the line from the previous ball position to the new position
* hits something, which indicates that the ball is tunneling through. If
* this happens, the ball position is adjusted so that it is just before
* the hit point. If tunneling happens four frames in a row the ball is
* considered stuck and explodes (e.g. the ball might try to tunnel through
* a wall to get to a 'close' target. In this case the ball would not
* move much anymore and be stuck).
* \return True if the ball tunneled often enough to be removed.
*/
bool RubberBall::checkTunneling()
{
const TriangleMesh &tm = World::getWorld()->getTrack()->getTriangleMesh();
Vec3 hit_point;
const Material *material;
tm.castRay(m_previous_xyz, getXYZ(), &hit_point, &material);
if(material)
{
// If there are three consecutive tunnelling
m_tunnel_count++;
if(m_tunnel_count > 3)
{
#ifdef PRINT_BALL_REMOVE_INFO
Log::debug("RubberBall",
"Ball %d nearly tunneled at %f %f %f -> %f %f %f",
m_id, m_previous_xyz.getX(),m_previous_xyz.getY(),
m_previous_xyz.getZ(),
getXYZ().getX(),getXYZ().getY(),getXYZ().getZ());
#endif
hit(NULL);
return true;
}
// In case of a hit, move the hit point towards the
// previous point by the radius of the ball --> this
// point will just allow the ball to avoid tunneling.
Vec3 diff = m_previous_xyz - hit_point;
hit_point += diff * (1.1f*m_extend.getY()/diff.length());
setXYZ(hit_point);
return false;
}
else
m_tunnel_count = 0;
return false;
} // checkTunneling
void RigidBody::setZ(const float z) {
setXYZ(getX(), getY(), z);
}
void RigidBody::setY(const float y) {
setXYZ(getX(), y, getZ());
}
void RigidBody::setX(const float x) {
setXYZ(x, getY(), getZ());
}
//Read the string and execute instructions
void process_string(uint8_t *instruction) {
uint8_t code;
uint16_t k;
float temp;
//command commands = NULL;
FloatPoint fp;
//the character / means delete block... used for comments and stuff.
if (instruction[0] == '/') {
Serial.println("ok");
return;
}
enable_steppers();
purge_commands(); //clear old commands
parse_commands(instruction); //create linked list of arguments
if (command_exists('G')) {
code = getValue('G');
switch(code) {
case 0: //Rapid Motion
setXYZ(&fp);
set_target(&fp);
r_move(0); //fast motion in all axis
break;
case 1: //Coordinated Motion
setXYZ(&fp);
set_target(&fp);
if (command_exists('F')) _feedrate = getValue('F'); //feedrate persists till changed.
r_move( _feedrate );
break;
case 2://Clockwise arc
case 3://Counterclockwise arc
FloatPoint cent;
float angleA, angleB, angle, radius, length, aX, aY, bX, bY;
//Set fp Values
setXYZ(&fp);
// Centre coordinates are always relative
cent.x = xaxis->current_units + getValue('I');
cent.y = yaxis->current_units + getValue('J');
aX = (xaxis->current_units - cent.x);
aY = (yaxis->current_units - cent.y);
bX = (fp.x - cent.x);
bY = (fp.y - cent.y);
if (code == 2) { // Clockwise
angleA = atan2(bY, bX);
angleB = atan2(aY, aX);
}
else { // Counterclockwise
angleA = atan2(aY, aX);
angleB = atan2(bY, bX);
}
// Make sure angleB is always greater than angleA
// and if not add 2PI so that it is (this also takes
// care of the special case of angleA == angleB,
// ie we want a complete circle)
if (angleB <= angleA) angleB += 2 * M_PI;
angle = angleB - angleA;
radius = sqrt(aX * aX + aY * aY);
length = radius * angle;
int steps, s, step;
steps = (int) ceil(length / curve_section);
FloatPoint newPoint;
for (s = 1; s <= steps; s++) {
step = (code == 3) ? s : steps - s; // Work backwards for CW
newPoint.x = cent.x + radius * cos(angleA + angle * ((float) step / steps));
newPoint.y = cent.y + radius * sin(angleA + angle * ((float) step / steps));
newPoint.z = zaxis->current_units;
set_target(&newPoint);
// Need to calculate rate for each section of curve
feedrate_micros = (feedrate > 0) ? feedrate : getMaxFeedrate();
// Make step
r_move(feedrate_micros);
}
break;
case 4: //Dwell
//delay((int)getValue('P'));
break;
case 20: //Inches for Units
_units[0] = X_STEPS_PER_INCH;
_units[1] = Y_STEPS_PER_INCH;
_units[2] = Z_STEPS_PER_INCH;
curve_section = CURVE_SECTION_INCHES;
calculate_deltas();
break;
case 21: //mm for Units
_units[0] = X_STEPS_PER_MM;
_units[1] = Y_STEPS_PER_MM;
_units[2] = Z_STEPS_PER_MM;
curve_section = CURVE_SECTION_MM;
calculate_deltas();
break;
case 28: //go home.
set_target(&zeros);
r_move(getMaxFeedrate());
break;
case 30://go home via an intermediate point.
//Set Target
setXYZ(&fp);
set_target(&fp);
//go there.
r_move(getMaxFeedrate());
//go home.
set_target(&zeros);
r_move(getMaxFeedrate());
break;
case 81: // drilling operation
temp = zaxis->current_units;
//Move only in the XY direction
setXYZ(&fp);
set_target(&fp);
zaxis->target_units = temp;
calculate_deltas();
r_move(getMaxFeedrate());
//Drill DOWN
zaxis->target_units = getValue('Z') + ((abs_mode) ? 0 : zaxis->current_units);
calculate_deltas();
r_move(getMaxFeedrate());
//Drill UP
zaxis->target_units = temp;
calculate_deltas();
r_move(getMaxFeedrate());
case 90://Absolute Positioning
abs_mode = true;
break;
case 91://Incremental Positioning
abs_mode = false;
break;
case 92://Set as home
set_position(&zeros);
break;
case 93://Inverse Time Feed Mode
break; //TODO: add this
case 94://Feed per Minute Mode
break; //TODO: add this
default:
Serial.print("huh? G");
Serial.println(code,DEC);
}
}
if (command_exists('M')) {
code = getValue('M');
switch(code) {
case 3: // turn on motor
case 4:
motor_on();
break;
case 5: // turn off motor
motor_off();
break;
case 82:
DDRC |= _BV(1);
PORTC &= ~_BV(1);
DDRC &= ~_BV(0);
PORTC |= _BV(0);
// setup initial position
for (int i=0; i<20; i++) {
k=0;
PORTB |= _BV(5); //go down
while(PINC & _BV(0)) {
PORTB |= _BV(2);
delayMicroseconds(1);
PORTB &= ~_BV(2);
delayMicroseconds(200);
k++;
}
//print result for this point
Serial.println(k,DEC);
PORTB &= ~_BV(5); //move up to origin
while (k--) {
PORTB |= _BV(2);
delayMicroseconds(1);
PORTB &= ~_BV(2);
delayMicroseconds(12.5*stepping);
}
}
break;
case 81:
DDRC |= _BV(1);
PORTC &= ~_BV(1);
DDRC &= ~_BV(0);
PORTC |= _BV(0);
while(1) {
if (PINC & _BV(0)) Serial.println("high");
else Serial.println("low");
}
break;
case 80: //plot out surface of milling area
DDRC |= _BV(1);
PORTC &= ~_BV(1);
DDRC &= ~_BV(0);
PORTC |= _BV(0);
// setup initial position
fp.x = 0;
fp.y = 0;
fp.z = 0;
set_target(&fp);
set_position(&fp);
r_move(0);
for (int i=0; i<160; i+=2) {
for (float j=0; j<75; j+=2) {
fp.x=i;
fp.y=j;
fp.z=0;
set_target( &fp );
r_move( 0 );
k=0;
PORTB &= ~(_BV(5)); //go down
while(PINC & _BV(0)) {
PORTB |= _BV(2);
delayMicroseconds(1);
PORTB &= ~_BV(2);
delayMicroseconds(200);
k++;
}
//print result for this point
Serial.print(i,DEC);
Serial.print(",");
Serial.print(j,DEC);
Serial.print(",");
Serial.println(k,DEC);
PORTB |= _BV(5); //move up to origin
while (k--) {
PORTB |= _BV(2);
delayMicroseconds(1);
PORTB &= ~_BV(2);
delayMicroseconds(200);
}
}
}
break;
case 90: //plot out surface of milling area
DDRC |= _BV(1);
PORTC &= ~_BV(1);
DDRC &= ~_BV(0);
PORTC |= _BV(0);
// setup initial position
fp.x = 0;
fp.y = 135;
fp.z = 0;
set_target(&fp);
set_position(&fp);
r_move(0);
for (int i=0; i<1; i++) {
for (float j=135; j!=0; j-=.25) {
fp.x=i;
fp.y=j;
fp.z=0;
set_target( &fp );
r_move( 0 );
k=0;
PORTB |= _BV(5); //go down
while(PINC & _BV(0)) {
PORTB |= _BV(2);
delayMicroseconds(1);
PORTB &= ~_BV(2);
delayMicroseconds(200);
k++;
}
//print result for this point
Serial.print(i,DEC);
Serial.print(",");
Serial.print(j,DEC);
Serial.print(",");
Serial.println(k,DEC);
PORTB &= ~_BV(5); //move up to origin
while (k--) {
PORTB |= _BV(2);
delayMicroseconds(1);
PORTB &= ~_BV(2);
delayMicroseconds(200);
}
}
}
break;
case 98: //M98 Find Z0 where it is one step from touching.
DDRC |= _BV(1);
PORTC &= ~_BV(1);
DDRC &= ~_BV(0);
PORTC |= _BV(0);
PORTB |= _BV(5);
while(PINC & _BV(0)) {
PORTB |= _BV(2);
delayMicroseconds(1);
PORTB &= ~_BV(2);
delayMicroseconds(200);
}
break;
case 99: //M99 S{1,2,4,8,16} -- set stepping mode
if (command_exists('S')) {
code = getValue('S');
if (code == 1 || code == 2 || code == 4 || code == 8 || code == 16) {
stepping = code;
setStep(stepping);
break;
}
}
default:
Serial.print("huh? M");
Serial.println(code,DEC);
}
}
Serial.println("ok");//tell our host we're done.
}
/**
* Fixe les coordonnées d'un vertex et met à jour l'array openGL.
*
* @param i
* @param nouveau
*/
void Mesh::setXYZ(int i, const QVector3D &nouveau)
{
setXYZ(i, nouveau.x(), nouveau.y(), nouveau.z());
}
VfxOffset::VfxOffset(float x, float y, float z) {
setXYZ(x, y, z);
}
ComCoordinate::ComCoordinate(double x, double y, double z, double w, double scale) {
setXYZ(x, y, z, w, scale);
}
void ComCoordinate::operator = (const ComCoordinate& c) {
setXYZ(c.x, c.y, c.z, c.w, c.scale);
}
/** Updates the rubber ball.
* \param dt Time step size.
* \returns True if the rubber ball should be removed.
*/
bool RubberBall::updateAndDelete(float dt)
{
LinearWorld *world = dynamic_cast<LinearWorld*>(World::getWorld());
// FIXME: what does the rubber ball do in case of battle mode??
if(!world) return true;
if(m_delete_timer>0)
{
m_delete_timer -= dt;
if(m_delete_timer<=0)
{
hit(NULL);
#ifdef PRINT_BALL_REMOVE_INFO
Log::debug("RubberBall", "ball %d deleted.", m_id);
#endif
return true;
}
}
// Update the target in case that the first kart was overtaken (or has
// finished the race).
computeTarget();
updateDistanceToTarget();
// Determine the new position. This new position is only temporary,
// since it still needs to be adjusted for the height of the terrain.
Vec3 next_xyz;
if(m_aiming_at_target)
moveTowardsTarget(&next_xyz, dt);
else
interpolate(&next_xyz, dt);
// If the ball is close to the ground, we have to start the raycast
// slightly higher (to avoid that the ball tunnels through the floor).
// But if the ball is close to the ceiling of a tunnel and we would
// start the raycast slightly higher, the ball might end up on top
// of the ceiling.
// The ball is considered close to the ground if the height above the
// terrain is less than half the current maximum height.
bool close_to_ground = 2.0*m_previous_height < m_current_max_height;
float vertical_offset = close_to_ground ? 4.0f : 2.0f;
// Note that at this stage getHoT still reports the height at
// the previous location (since TerrainInfo wasn't updated). On
// the other hand, we can't update TerrainInfo without having
// at least a good estimation of the height.
next_xyz.setY(getHoT() + vertical_offset);
// Update height of terrain (which isn't done as part of
// Flyable::update for rubber balls.
TerrainInfo::update(next_xyz);
m_height_timer += dt;
float height = updateHeight()+m_extend.getY()*0.5f;
float new_y = getHoT()+height;
if(UserConfigParams::logFlyable())
printf("ball %d: %f %f %f height %f new_y %f gethot %f ",
m_id, next_xyz.getX(), next_xyz.getY(), next_xyz.getZ(), height, new_y, getHoT());
// No need to check for terrain height if the ball is low to the ground
if(height > 0.5f)
{
float terrain_height = getMaxTerrainHeight(vertical_offset)
- m_extend.getY();
if(new_y>terrain_height)
new_y = terrain_height;
}
if(UserConfigParams::logFlyable())
Log::verbose("RubberBall", "newy2 %f gmth %f", new_y,
getMaxTerrainHeight(vertical_offset));
next_xyz.setY(new_y);
m_previous_xyz = getXYZ();
m_previous_height = next_xyz.getY()-getHoT();
setXYZ(next_xyz);
if(checkTunneling())
return true;
// Determine new distance along track
TrackSector::update(next_xyz);
// Ball squashing:
// ===============
if(height<1.5f*m_extend.getY())
m_node->setScale(core::vector3df(1.0f, height/m_extend.getY(),1.0f));
else
m_node->setScale(core::vector3df(1.0f, 1.0f, 1.0f));
return Flyable::updateAndDelete(dt);
} // updateAndDelete
Vector3D::Vector3D(void)
{
setXYZ(0, 0, 0);
}
/* Smart CDF helper routine that return three values */
void cdf_Q_helper(enum nilop op) {
decNumber a, b, t, u, x2, d, absx, x;
int i;
getX(&x);
dn_abs(&absx, &x);
if (dn_lt(&absx, &const_2_326)) {
decNumberSquare(&x2, &absx);
decNumberCopy(&t, &absx);
decNumberCopy(&a, &absx);
decNumberCopy(&d, &const_3);
for (i=0;i<500; i++) {
dn_multiply(&u, &t, &x2);
dn_divide(&t, &u, &d);
dn_add(&u, &a, &t);
if (dn_eq(&u, &a))
break;
decNumberCopy(&a, &u);
dn_p2(&d, &d);
}
decNumberCopy(&b, &const_0_5);
if (decNumberIsNegative(&x))
dn_minus(&a, &a);
} else {
const decNumber *nom, *extra, *sub;
//dn_minus(&x2, &absx);
//n = ceil(extra + nom / (|x| - sub))
if (is_usrdblmode()) {
sub = &const_1_5;
nom = &const_300;
extra = &const_8;
}
else {
sub = &const_1_3;
nom = &const_100;
extra = &const_4;
}
dn_subtract(&b, &absx, sub);
dn_divide(&t, nom, &b);
dn_add(&u, &t, extra);
decNumberCeil(&b, &u);
decNumberZero(&t);
do {
dn_add(&u, &x, &t);
dn_divide(&t, &b, &u);
dn_dec(&b);
} while (! dn_eq0(&b));
dn_add(&u, &t, &x);
decNumberRecip(&a, &u);
if (decNumberIsNegative(&a)) {
dn_minus(&a, &a);
decNumberZero(&b);
} else {
dn_1(&b);
dn_minus(&a, &a);
}
}
pdf_Q(&t, &x);
setXYZ(&t, &a, &b);
}
