/* ************************************************************************* */
Rot3 Rot3::Random(boost::mt19937& rng) {
// TODO allow any engine without including all of boost :-(
Unit3 axis = Unit3::Random(rng);
boost::uniform_real<double> randomAngle(-M_PI, M_PI);
double angle = randomAngle(rng);
return AxisAngle(axis, angle);
}
bool circleEmitter::addParticle(){
particle *newParticle;
float speed;
//Particle pool exists and max num particles not exceeded
if(e != NULL && *managerParticleList != NULL && e->particleCount < e->totalParticles && emitting){
newParticle = *managerParticleList;
*managerParticleList = (*managerParticleList)->next;
if(e->particleList != NULL){
e->particleList->prev = newParticle;
}
newParticle->next = e->particleList;
newParticle->prev = NULL;
e->particleList = newParticle;
float angle = randomAngle();
float radScalar = randDist();
newParticle->rand = radScalar;
newParticle->radius = radius * radScalar;
STVector3 point = STVector3(radius*radScalar*cosf(angle), 0, radius*radScalar*sinf(angle));
STVector3 straightUp = STVector3(0,1,0);
STVector3 circleDir = STVector3(e->dir.x, e->dir.y, e->dir.z);
STVector3 a = STVector3::Cross(straightUp, circleDir);
float w = sqrt(powf(straightUp.Length(), 2) * powf(circleDir.Length(), 2)) + STVector3::Dot(straightUp, circleDir);
Quaternion rotateCircle = Quaternion(w, a.x, a.y, a.z);
rotateCircle.Normalize();
STVector3 rotatedPoint = rotateCircle.rotate(point, rotateCircle);
newParticle->pos.x = rotatedPoint.x + e->pos.x;
newParticle->pos.y = rotatedPoint.y + e->pos.y;
newParticle->pos.z = rotatedPoint.z + e->pos.z;
/*
newParticle->pos.x = e->pos.x + radius*sinf(angle);
newParticle->pos.y = e->pos.y;
newParticle->pos.z = e->pos.z + radius*cosf(angle);
*/
newParticle->prevPos.x = 0;
newParticle->prevPos.y = 0;
newParticle->prevPos.z = 0;
newParticle->dir = e->dir + (e->dirVar*randDist());
speed = e->speed + (e->speed * randDist());
newParticle->dir.x *= speed;
newParticle->dir.y *= speed;
newParticle->dir.z *= speed;
newParticle->life = e->life + (int)((float)e->lifeVar * randDist());
newParticle->side = randDist();
e->particleCount++;
return true;
}
return false;
}
void Docking::NachoptimierungR(Protein* p,int score,Vector3 shift_back, System s1){
Protein copy1(*p, true);
Protein* copy = ©1;
TranslationProcessor translation;
Vector3 toOrigin = shift_back*(-1);
translation.setTranslation(toOrigin);
copy->apply(translation);
srand(time(NULL));
Matrix4x4 randomMatrix;
//nachoptimierung nur minimal da score schon gut
float angle = 1 + rand() % (10 - 1 + 1);
Angle randomAngle(angle, false);
//Random Vektor als rotationsaxe
int min = -3;
int max = 3;
float x = min + rand() % (max - min +1);
float y = min + rand() % (max - min +1);
float z = min + rand() % (max - min +1);
Vector3 vec(x,y,z);
randomMatrix.setRotation(randomAngle,vec);
TransformationProcessor randomTransformation(randomMatrix);
copy->apply(randomTransformation);
TranslationProcessor translation2;
translation2.setTranslation(shift_back);
copy->apply(translation2);
vector<Vector3> position = data.savePositions(copy);
float newscore = scoring(position);
//nur wenn der score höher ist interessierr er uns
if(newscore > score){
System opt;
opt.insert(*copy);
data.writeFinalComplex(s1, opt, score, newscore);
}
}
void MountainAgent::doWork() {
float direction = M_PI/4;
float addDirection = M_PI/4;
float _x = point.first;
float _y = point.second;
int chngCount = changeDirc;
srandom((unsigned int) time(NULL));
while(tokens--){
if(chngCount==0){
float d = random()%100+1;
direction += randomAngle(-M_PI/4, M_PI/4);
//std::cerr<< d <<"\n";
chngCount = changeDirc;
}
direction += randomAngle(-M_PI/100,M_PI/100);
chngCount--;
int countLoop = 0;
//std::cerr<<image.rows<<" "<<image.cols<<"\n";
while(_x >= image.cols || _y >= image.rows || _x < 0 || _y < 0 || isBG.at<uchar>(_y, _x) == 1){
_x = random()%image.cols;
_y = random()%image.rows;
countLoop++;
if(countLoop > image.rows * image.cols)return;
}
int x = (int) _x, y = (int) _y;
assert(_x >=0 && _y >= 0 && x < image.cols && y < image.rows);
image.at<uchar>(y, x) = bgColor;
_x += sinf(direction);
_y += cosf(direction);
}
}
static void
check_quaternion(generator_t &gen, distribution_t &dist)
{
// check identity
{
quat q;
glm::quat g;
quatIdentity(&q);
assert(q == g);
}
for(size_t x = 0; x < 10000; ++x)
{
// check negation
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
assert(quatNegate(q) == -g);
}
// check addition
{
quat q1 = randomQuat(gen, dist);
quat q2 = randomQuat(gen, dist);
glm::quat g1 = loadQuat(q1);
glm::quat g2 = loadQuat(q2);
assert(quatAdd(q1, q2) == g1+g2);
}
// check subtraction
{
quat q1 = randomQuat(gen, dist);
quat q2 = randomQuat(gen, dist);
glm::quat g1 = loadQuat(q1);
glm::quat g2 = loadQuat(q2);
assert(quatSubtract(q1, q2) == g1 + (-g2));
}
// check scale
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
float f = dist(gen);
assert(quatScale(q, f) == g*f);
}
// check normalize
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
assert(quatNormalize(q) == glm::normalize(g));
}
// check dot
{
quat q1 = randomQuat(gen, dist);
quat q2 = randomQuat(gen, dist);
glm::quat g1 = loadQuat(q1);
glm::quat g2 = loadQuat(q2);
assert(std::abs(quatDot(q1, q2) - glm::dot(g1, g2)) < 0.0001f);
}
// check conjugate
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
assert(quatConjugate(q) == glm::conjugate(g));
}
// check inverse
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
assert(quatInverse(q) == glm::inverse(g));
}
// check quaternion multiplication
{
quat q1 = randomQuat(gen, dist);
quat q2 = randomQuat(gen, dist);
glm::quat g1 = loadQuat(q1);
glm::quat g2 = loadQuat(q2);
assert(quatMultiply(q1, q2) == g1*g2);
}
// check vector multiplication
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
glm::vec3 v = randomVector(gen, dist);
assert(quatMultiplyVec3f(q, (vec3f){ v.x, v.y, v.z }) == g*v);
}
// check rotation
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
glm::vec3 v = randomVector(gen, dist);
float r = randomAngle(gen, dist);
assert(quatRotate(q, (vec3f){ v.x, v.y, v.z }, r) == glm::rotate(g, r, v));
}
// check rotate X
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
float r = randomAngle(gen, dist);
assert(quatRotateX(q, r) == glm::rotate(g, r, x_axis));
}
// check rotate Y
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
float r = randomAngle(gen, dist);
assert(quatRotateY(q, r) == glm::rotate(g, r, y_axis));
}
// check rotate Z
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
float r = randomAngle(gen, dist);
assert(quatRotateZ(q, r) == glm::rotate(g, r, z_axis));
}
// check conversion from matrix
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
mtx44 m;
quatToMtx44(&m, q);
assert(m == glm::mat4_cast(g));
}
}
}
static void
check_matrix(generator_t &gen, distribution_t &dist)
{
// check identity
{
mtx44 m;
mtx44Identity(&m);
assert(m == glm::mat4());
}
for(size_t x = 0; x < 10000; ++x)
{
// check multiply
{
mtx44 m1, m2;
randomMatrix(m1, gen, dist);
randomMatrix(m2, gen, dist);
glm::mat4 g1 = loadMatrix(m1);
glm::mat4 g2 = loadMatrix(m2);
mtx44 result;
mtx44Multiply(&result, &m1, &m2);
assert(result == g1*g2);
}
// check translate
{
mtx44 m;
randomMatrix(m, gen, dist);
glm::mat4 g = loadMatrix(m);
glm::vec3 v = randomVector(gen, dist);
mtx44Translate(&m, v.x, v.y, v.z);
assert(m == glm::translate(g, v));
}
// check scale
{
mtx44 m;
randomMatrix(m, gen, dist);
glm::mat4 g = loadMatrix(m);
glm::vec3 v = randomVector(gen, dist);
mtx44Scale(&m, v.x, v.y, v.z);
assert(m == glm::scale(g, v));
}
// check rotate
{
mtx44 m;
randomMatrix(m, gen, dist);
float r = randomAngle(gen, dist);
glm::mat4 g = loadMatrix(m);
glm::vec3 v = randomVector(gen, dist);
mtx44Rotate(&m, (vec3f){ v.x, v.y, v.z }, r);
assert(m == glm::rotate(g, r, v));
}
// check rotate X
{
mtx44 m;
randomMatrix(m, gen, dist);
float r = randomAngle(gen, dist);
glm::mat4 g = loadMatrix(m);
mtx44RotateX(&m, r);
assert(m == glm::rotate(g, r, x_axis));
}
// check rotate Y
{
mtx44 m;
randomMatrix(m, gen, dist);
float r = randomAngle(gen, dist);
glm::mat4 g = loadMatrix(m);
mtx44RotateY(&m, r);
assert(m == glm::rotate(g, r, y_axis));
}
// check rotate Z
{
mtx44 m;
randomMatrix(m, gen, dist);
float r = randomAngle(gen, dist);
glm::mat4 g = loadMatrix(m);
mtx44RotateZ(&m, r);
assert(m == glm::rotate(g, r, z_axis));
}
}
}
// ============================================================================================
// switches between all the sensor modes
// if behavior has been altered by sensor interaction returns true (except RATE)
// ============================================================================================
bool sensorBehavior(){
bool updated = false;
if (presMode != lastPresMode){
if (presenceDetected && !ignorePresence){
// smooth transition when changing presence modes
fprintf_P(&usart_stream, PSTR("changePresMode\r\n"));
startXFade(0.01);
}
switch(presMode){
case POINT:
presenceTimeOut = 6; // in seconds
neighborPresenceTimeOut = presenceTimeOut;
usingNeighborPresence = true;
break;
case RANDOM:
presenceTimeOut = 8;
neighborPresenceTimeOut = presenceTimeOut;
usingNeighborPresence = false;
break;
case RATE:
presenceTimeOut = 20;
neighborPresenceTimeOut = presenceTimeOut;
usingNeighborPresence = false;
break;
case SHAKE:
presenceTimeOut = 8;
neighborPresenceTimeOut = presenceTimeOut;
usingNeighborPresence = false;
break;
case WAVE:
presenceTimeOut = 4;
neighborPresenceTimeOut = presenceTimeOut;
usingNeighborPresence = true;
break;
case RHYTHM:
presenceTimeOut = 40;
neighborPresenceTimeOut = presenceTimeOut;
usingNeighborPresence = false;
break;
}
}
// because for rate there is no change of angle just update rate so don't need to transition
if(newPresence && presMode != RATE && presMode != IGNORE){
fprintf_P(&usart_stream, PSTR("newPres\r\n"));
startXFade(0.1);
}
if (newNeighborPresence && usingNeighborPresence){
newNeighborPresence = false;
switch(presMode){
case POINT:
fprintf_P(&usart_stream, PSTR("new neighbor\r\n"));
if (neighborPresenceDetected)
startXFade(0.05);
else
startXFade(0.01);
break;
}
}
switch(presMode)
{
case POINT:
updated = point();
break;
case SHAKE:
updated = presenceWave(false);
break;
case WAVE:
updated = presenceWave(true);
break;
case RANDOM:
updated = randomAngle();
break;
case RATE:
updated = randomUpdateRate();
break;
case RHYTHM:
updated = toFro();
case IGNORE: // do nothing
break;
}
return updated;
}
int main (void)
{
uint8_t leds_cnt = 99;
uint8_t leds_state = 0;
uint8_t leds_on = 1;
int16_t turn_angle = 0;
uint8_t turn_dir = 1;
uint8_t turning = 0;
uint8_t backing_up = 0;
// Set up Create and module
initialize();
LEDBothOff;
powerOnRobot();
byteTx(CmdStart);
baud(Baud28800);
defineSongs();
byteTx(CmdControl);
byteTx(CmdFull);
// Stop just as a precaution
patrol_room(0, RadStraight);
// Play the reset song and wait while it plays
byteTx(CmdPlay);
byteTx(RESET_SONG);
delayAndUpdateSensors(750);
for(;;)
{
if(++leds_cnt >= 100)
{
leds_cnt = 0;
leds_on = !leds_on;
if(leds_on)
{
byteTx(CmdLeds);
byteTx(LEDsBoth);
byteTx(128);
byteTx(255);
LEDBothOff;
}
else
{
byteTx(CmdLeds);
byteTx(0x00);
byteTx(0);
byteTx(0);
LEDBothOn;
}
}
delayAndUpdateSensors(10);
if(UserButtonPressed)
{
// Play start song and wait
byteTx(CmdPlay);
byteTx(START_SONG);
delayAndUpdateSensors(2813);
// Drive around until a button or unsafe condition is detected
while(!(UserButtonPressed)
&& (!sensors[SenCliffL])
&& (!sensors[SenCliffFL])
&& (!sensors[SenCliffFR])
&& (!sensors[SenCliffR])
&& (!sensors[SenChAvailable])
)
{
// Keep turning until the specified angle is reached
if(turning)
{
if(backing_up)
{
if((-distance) > 5)
backing_up = 0;
patrol_room(-200, RadStraight);
}
else
{
if(turn_dir)
{
if(angle > turn_angle)
turning = 0;
patrol_room(200, RadCCW);
}
else
{
if((-angle) > turn_angle)
turning = 0;
patrol_room(200, RadCW);
}
}
}
else if(sensors[SenBumpDrop] & BumpEither) // Check for a bump
{
// Set the turn parameters and reset the angle
if(sensors[SenBumpDrop] & BumpLeft)
turn_dir = 0;
else
turn_dir = 1;
backing_up = 1;
turning = 1;
distance = 0;
angle = 0;
turn_angle = randomAngle();
// Play the bump song
byteTx(CmdPlay);
byteTx(BUMP_SONG);
}
else
{
// Otherwise, patrol_room straight
patrol_room(300, RadStraight);
}
// Flash the leds in sequence
if(++leds_cnt >= 10)
{
leds_cnt = 0;
if(turning)
{
// Flash backward while turning
if(leds_state == 0)
leds_state = 4;
else
leds_state--;
}
else
{
if(leds_state == 4)
leds_state = 0;
else
leds_state++;
}
if(leds_state == 0)
{
// robot Power LED Amber
byteTx(CmdLeds);
byteTx(0x00);
byteTx(128);
byteTx(255);
LEDBothOff;
}
else if(leds_state == 1)
{
// Play LED on
byteTx(CmdLeds);
byteTx(LEDPlay);
byteTx(0);
byteTx(0);
LEDBothOff;
}
else if(leds_state == 2)
{
// Advance LED on
byteTx(CmdLeds);
byteTx(LEDAdvance);
byteTx(0);
byteTx(0);
LEDBothOff;
}
else if(leds_state == 3)
{
// Robot LEDs off, CM left LED on
byteTx(CmdLeds);
byteTx(0x00);
byteTx(0);
byteTx(0);
LED2On;
LED1Off;
}
else if(leds_state == 4)
{
// Robot LEDs off, CM right LED on
byteTx(CmdLeds);
byteTx(0x00);
byteTx(0);
byteTx(0);
LED1On;
LED2Off;
}
}
// wait a little more than one robot tick for sensors to update
delayAndUpdateSensors(20);
}
// Stop driving
patrol_room(0, RadStraight);
// Play end song and wait
delayAndUpdateSensors(500);
byteTx(CmdPlay);
byteTx(END_SONG);
delayAndUpdateSensors(2438);
}
}
}
