void Ball::Update()
{
calcSpeedXY();
x += vx;
y += vy;
//bounce
if (x - radius < range_left)
{
vx = std::abs(vx);
calcAngle();
}
else if (x + radius >= range_right)
{
vx = -std::abs(vx);
calcAngle();
}
if (y - radius < range_top)
{
vy = std::abs(vy);
calcAngle();
}
else if (y + radius >= range_bottom)
{
vy = -std::abs(vy);
calcAngle();
}
adjustPosition();
}
LocationVector MMapManager::getBestPositionOnPathToLocation(float startx, float starty, float startz, float endx, float endy, float endz)
{
LocationVector pos(startx, starty, startz);
LocationVector nextpos(startx, starty, startz);
LocationVector returnpos(endx, endy, endz);
pos = getNextPositionOnPathToLocation(startx, starty, startz, endx, endy, endz);
float line = calcAngle(startx, starty, pos.x, pos.y);
while(1)
{
nextpos = getNextPositionOnPathToLocation(pos.x, pos.y, pos.z, endx, endy, endz);
float angle = calcAngle( startx, starty, nextpos.x, nextpos.y );
if(angle != line)
{ // We have to turn, so stop our line here.
returnpos = pos;
break;
}
if(pos.x == nextpos.x || pos.y == nextpos.y)
{
returnpos = pos;
break;
}
pos = nextpos;
}
return returnpos;
}
int main()
{
printf("%d \n", calcAngle(6, 180));
printf("%d \n", calcAngle(3, 90));
printf("%d \n", calcAngle(9, 90));
return 0;
}
void Magnet::pull(Entity* b)
{
double distance = distBetween(center, ((Ball*)b)->getCenter());
if(distance > maxDistance)
return;
double force = (magneticForce - (magneticForce * (distance - getRadius() - b->getRadius()) / maxDistance));
double angle = calcAngle(b->getCenter(),getCenter());
double xForce = ( force * cos(angle));
double yForce = ( force * sin(angle));
if(!linearForce)
{
int sign = 1;
if(xForce < 0)
sign = -1;
xForce = xForce * xForce * sign;
sign = 1;
if(yForce < 0)
sign = -1;
yForce = yForce * yForce * sign;
}
((Ball*)b)->setXVel( ((Ball*)b)->getXVel() + xForce * window->timedMovement());
((Ball*)b)->setYVel( ((Ball*)b)->getYVel() + yForce * window->timedMovement());
((Ball*)b)->movedByMagnet();
}
void BasicVFF::run(const GridMap & map)
{
calcRepulsiveForceSum(map);
calcDirection();
calcAngle();
calcSteerRate();
}
void LineScatterer::paint ( QGraphicsScene * scene, double xMin, double xMax, double yMin, double yMax ) {
//QLOG_DEBUG() << "LineScatterer::paint";
innerState->form.show();
state->xMax = xMax;
state->yMax = yMax;
state->xMin = xMin;
state->yMin = yMin;
state->scene = scene;
double x1Scene = getXscene( innerState->x1, xMin, xMax, scene->width() ) - state->posX;
double y1Scene = getYscene( innerState->y1, yMin, yMax, scene->height() ) - state->posY;
double x2Scene = getXscene( innerState->x2, xMin, xMax, scene->width() ) - state->posX;
double y2Scene = getYscene( innerState->y2, yMin, yMax, scene->height() ) - state->posY;
QPainterPath path_;
path_.moveTo( x1Scene, y1Scene );
path_.lineTo( x2Scene, y2Scene );
setPath( path_ );
setTransformOriginPoint( (x1Scene + x2Scene) / 2, ( y1Scene + y2Scene ) / 2 ); //posX, posY - because of mouse drag
// setRotation( state->rotationAngle );
double angle = calcAngle();
//QLOG_INFO() << "angle = " << angle;
if ( !state->isAdded ) {
scene->addItem( this );
state->isAdded = true;
}
state->boundingRectBeforeDrag = sceneBoundingRect();
}
/* ---------------------------------
Jacobi
---------------------------------- */
void jacobi( Matrix *m, int num_of_file ) {
Matrix *eigen_values = m
, *eigen_vectors = generateIdentityMatrix( m->height )
, old
;
Coordinate position;
char fname[256];
double theta;
int count = 0;
while(1) {
position = locateMaxValue( eigen_values );
if ( position.value < THRESHOLD )
break;
printf("%d回目 : %lf\n", count++, position.value );
theta = calcAngle( m, position );
eigen_values = rotate( eigen_values, position, theta );
eigen_vectors = append( eigen_vectors, position, theta );
repair( eigen_values );
}
sprintf( fname, "EigenValues%02d.txt", num_of_file );
printMatrixToFile( fname, eigen_values );
sprintf( fname, "EigenVectors%02d.txt", num_of_file );
printMatrixToFile( fname, eigen_vectors );
}
void navigateToWaypoint(float xtarget, float ytarget)
{
while(!atTarget(xtarget, x) || (!atTarget(ytarget, y)))
{
float distance = calcDistance(xtarget, ytarget);
float angle = calcAngle(xtarget, ytarget);
float driveDistance = 0; //initialise
if (distance < stepDistance)
{
driveDistance = distance;
}
else
{
driveDistance = stepDistance;
}
// co-ordinate system is different so cos and sin are swapped - take from current location not origin
float tempWaypointX = x + (driveDistance * cos(angle));
float tempWaypointY = y + (driveDistance * sin(angle));
driveToWaypoint(tempWaypointX, tempWaypointY);
monteCarlo();
eraseDisplay();
drawMap();
drawParticles();
wait1Msec(100);
}
PlayTone(784, 15);
}
/*****************************
//初始化变量，一些全局变量已经被初始化
*****************************/
void variable_init()
{
memset(&anglePID,NULL,sizeof(PID));
memset(&kPID,NULL,sizeof(PID));
memset(&bPID,NULL,sizeof(PID));
memset(&leftSpeedPID,NULL,sizeof(PID));
memset(&rightSpeedPID,NULL,sizeof(PID));
anglePID.P=600;//
anglePID.D=6; //
leftSpeedPID.P=100;//
rightSpeedPID.P=100;
leftSpeedPID.I=0;//
rightSpeedPID.I=0;
leftSpeedPID.D=10;//
rightSpeedPID.D=10;
kPID.P=10;//10
kPID.D=0;
bPID.P=32;
bPID.D=0;
setSpeed=0;
calcAngle();
theta2=theta1;
angle=theta1;
}
Foam::arcEdge::arcEdge(const pointField& points, Istream& is)
:
curvedEdge(points, is),
p1_(points_[start_]),
p2_(is),
p3_(points_[end_]),
cs_(calcAngle())
{}
polar_t convertRectangularToPolar( complex_t c )
{
polar_t p;
p.radius = calcRadius( c );
p.angle = calcAngle( c );
return p;
}
Transform simpleMotionsToTransform(TransformData* td,
const LocalMotions* motions){
int center_x = 0;
int center_y = 0;
Transform t = null_transform();
if(motions==0) return t;
int num_motions=vs_vector_size(motions);
double *angles = (double*) vs_malloc(sizeof(double) * num_motions);
LocalMotion meanmotion;
int i;
if(num_motions < 1)
return t;
// calc center point of all remaining fields
for (i = 0; i < num_motions; i++) {
center_x += LMGet(motions,i)->f.x;
center_y += LMGet(motions,i)->f.y;
}
center_x /= num_motions;
center_y /= num_motions;
// cleaned mean
meanmotion = cleanmean_localmotions(motions);
// figure out angle
if (num_motions < 6) {
// the angle calculation is inaccurate for 5 and less fields
t.alpha = 0;
} else {
for (i = 0; i < num_motions; i++) {
// substract avg and calc angle
LocalMotion m = sub_localmotion(LMGet(motions,i),&meanmotion);
angles[i] = calcAngle(&m, center_x, center_y);
}
double min, max;
t.alpha = -cleanmean(angles, num_motions, &min, &max);
if (max - min > td->maxAngleVariation) {
t.alpha = 0;
vs_log_info(td->modName, "too large variation in angle(%f)\n",
max-min);
}
}
vs_free(angles);
// compensate for off-center rotation
double p_x = (center_x - td->fiSrc.width / 2);
double p_y = (center_y - td->fiSrc.height / 2);
t.x = meanmotion.v.x + (cos(t.alpha) - 1) * p_x - sin(t.alpha) * p_y;
t.y = meanmotion.v.y + sin(t.alpha) * p_x + (cos(t.alpha) - 1) * p_y;
return t;
}
Foam::arcEdge::arcEdge
(
const pointField& points,
const label start,
const label end,
const point& pMid
)
:
curvedEdge(points, start, end),
p1_(points_[start_]),
p2_(pMid),
p3_(points_[end_]),
cs_(calcAngle())
{}
Spectator::Turn::Turn(engine::IClump* clump, Vector3f dir) :
Character::Action( clump )
{
// set action properties
_actionTime = 0.0f;
_blendTime = turnBlendTime;
_endOfAction = false;
_dir = dir;
// animation controller
engine::IAnimationController* controller = _clump->getAnimationController();
// capture blend source
controller->captureBlendSrc();
// reset animation mixer
for( unsigned int i=0; i<engine::maxAnimationTracks; i++ )
{
if( controller->getTrackAnimation( i ) ) controller->setTrackActivity( i, false );
}
// calculate angle between current direction & desired direction
float angle = calcAngle( _dir, _clump->getFrame()->getAt(), Vector3f(0,1,0) );
// choose animation cycle
if( sgn( angle ) < 0 )
{
controller->setTrackAnimation( 0, &turnRightSequence );
}
else
{
controller->setTrackAnimation( 0, &turnLeftSequence );
}
// setup animation mixing
controller->setTrackSpeed( 0, 1.0f );
controller->setTrackWeight( 0, 1.0f );
controller->resetTrackTime( 0 );
controller->setTrackActivity( 0, true );
// capture blend destination
controller->advance( 0.0f );
controller->captureBlendDst();
controller->blend( 0.0f );
}
// on intersection of spheres c1,r1 and c2,r2 find p such that p-c1-q is of certain value, and lies on intersection of 2 spheres
// this is solved by sampling points on the circle of intersection and checking the angle
int findSphereSphereAngleIntx(float r1, vector<float> & c1, float r2, vector<float> & c2, vector<float> & q, float desiredAngle,
vector<float>& p1, vector<float>& p2) {
vector<float> c1c2(3), Y(3), Z(3), c(3);
linear_combination(c1c2, 1, c2, -1, c1);
double cc = magnitude(c1c2);
cout << cc << endl;
if(cc > r1+r2) return 0;
normalize_point(c1c2, c1c2);
findYZgivenX(c1c2, Y, Z);
cout << "c1c2 " << c1c2[0] << " " << c1c2[1] << " " << c1c2[2] << endl;
cout << "Y " << Y[0] << " " << Y[1] << " " << Y[2] << endl;
cout << "Z " << Z[0] << " " << Z[1] << " " << Z[2] << endl;
double c1c = (r1*r1 + cc*cc - r2*r2) / (2*cc);
double r = sqrt(r1*r1 - c1c*c1c);
linear_combination(c, 1, c1, c1c, c1c2);
cout << "C " << c[0] << " " << c[1] << " " << c[2] << endl;
float lastAngle = -999, angle, lastTheta = -999;
float startTheta = 0, stopTheta = 360, step = 5; // clockwise
int nsol = 0;
vector<float> p(3);
for(float theta = startTheta; theta <= stopTheta; theta += step) {
double th = M_PI * theta / 180;
linear_combination(p, 1, c, r*sin(th), Y);
linear_combination(p, 1, p, r*cos(th), Z);
angle = calcAngle(q, c1, p);
cout << "P " << lastAngle << " " << desiredAngle << " " << angle << " " << p[0] << " " << p[1] << " " << p[2] << endl;
if(lastAngle != -999 && (desiredAngle-angle)*(desiredAngle-lastAngle) <= 0) {
double th = M_PI * (theta + lastTheta)/360.;
if(nsol==0) { linear_combination(p1, 1, c, r*sin(th), Y); linear_combination(p1, 1, p1, r*cos(th), Z); }
if(nsol==1) { linear_combination(p2, 1, c, r*sin(th), Y); linear_combination(p2, 1, p2, r*cos(th), Z); }
assert(nsol!=2);
nsol++;
cout << "NSOL " << nsol << endl;
}
lastAngle = angle;
lastTheta = theta;
}
return nsol;
}
void main() {
int sock; /* Socket descriptor */
struct sockaddr_in echoServAddr; /* Echo server address */
WSADATA wsaData; /* Structure for WinSock setup communication */
int bytesRecv = SOCKET_ERROR;
char sendbuf[32] = "Client: Sending data.";
char recvbuf[32] = "";
calcAngle(20, 20, 279, 105);
if(WSAStartup(MAKEWORD(2, 0), &wsaData) != 0) /* Load Winsock 2.0 DLL */
{
fprintf(stderr, "WSAStartup() failed");
exit(1);
}
/* Create a reliable, stream socket using TCP */
if ((sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0)
DieWithError("socket() failed");
/* Construct the server address structure */
memset(&echoServAddr, 0, sizeof(echoServAddr)); /* Zero out structure */
echoServAddr.sin_family = AF_INET; /* Internet address family */
echoServAddr.sin_addr.s_addr = inet_addr("127.0.0.1"); /* Server IP address */
echoServAddr.sin_port = htons(9001); /* Server port */
/* Establish the connection to the echo server */
if (connect(sock, (struct sockaddr *) &echoServAddr, sizeof(echoServAddr)) < 0)
DieWithError("connect() failed");
/* Send the TOS_Msg to the server */
/* if (send(sock, (char*) msg, sizeof(TOS_Msg), 0) != sizeof(TOS_Msg))
DieWithError("send() sent a different number of bytes than expected");*/
while( bytesRecv == SOCKET_ERROR ) {
bytesRecv = recv(sock, recvbuf, 32, 0 );
if ( bytesRecv == 0 || bytesRecv == WSAECONNRESET ) {
printf( "Connection Closed.\n");
break;
}
if (bytesRecv < 0)
return;
printf( "Bytes Recv: %d\n", bytesRecv );
}
bytesRecv = recv(sock, recvbuf, 32, 0 );
}
void Spectator::Turn::update(float dt)
{
updateAnimation( dt );
// leave if turn is starting
if( _actionTime - _blendTime < FRAMETIME(143) - FRAMETIME(135) ) return;
// evaluate end of action
float angle = calcAngle( _dir, _clump->getFrame()->getAt(), Vector3f( 0,1,0 ) );
if( fabs( angle ) < 1.0f )
{
_endOfAction = true;
return;
}
// evaluate angle to turn
float angleToTurn = sgn( angle ) * turnVelocity * dt;
if( fabs( angle ) < fabs( angleToTurn ) ) angleToTurn = angle;
// rotate frame
_clump->getFrame()->rotateRelative( Vector3f(0,1,0), angleToTurn );
}
/* ---------------------------------
Jacobi
---------------------------------- */
void jacobi( Matrix *m, int num_of_file ) {
Matrix *eigen_values = m
, *eigen_vectors = generateIdentityMatrix( m->height )
;
Coordinate position;
Trigonometry angle;
int count = 0
, not_change = 0
;
double previous = 0;
while(1) {
position = locateMaxValue( eigen_values );
if ( position.value < THRESHOLD )
break;
if ( position.value == previous )
not_change++;
else
not_change = 0;
if ( not_change == 100 )
break;
previous = position.value;
// printf("%d回目 : %lf\n", count++, position.value );
angle = calcAngle( eigen_values, position );
rotate( eigen_values, position, angle );
append( eigen_vectors, position, angle );
repair( eigen_values );
}
printEigen( eigen_values, eigen_vectors, num_of_file );
}
// if fwd, C,N,CA are C,N,CA, else assumed to be N,C,CA
void InformedPhipsiSampler::sample(
vector<float>& curC, vector<float>& curN, vector<float>& curCA,
float & phi, float & psi, float & omega, float & r, float & a, float & t) {
float d0 = calcDist(target, curCA);
float cisdist = 2.8, transdist = 3.81;
bool cis = false, trans = false;
if(cisdist > d0 - targetTol && cisdist < d0 + targetTol) cis = true;
if(transdist > d0 - targetTol && transdist < d0 + targetTol) trans = true;
if(cis && trans) { // if both cis and trans are possible, choose one
if(ran01() > 0.95) trans = false;
} else if(!cis && !trans) {
cout << "target unreachable "
<<curCA[0]<<" "<<curCA[1]<<" "<<curCA[2] << " : " << d0 <<" : "<< targetTol <<" : "<< target[0]<<" "<<target[1]<<" "<<target[2]
<< endl;
exit(0);
}
// now either cis or trans is true
float interCA;
if(cis) { omega = 0; interCA = cisdist; }
if(trans) { omega = -180; interCA = transdist; }
phi = -999; psi = -999;
// find a,t which take u into restraint sphere with this interCA
vector<float> tar(3,0), noi(3,0);
while(1) {
// add some noise to target, proportional to noise
randomNormalVector(noi);
linear_combination(tar, 1, target, targetTol*ran01(), noi);
// for this interCA, find a,t that takes you closest to target
r = calcDist(curCA, tar);
a = calcAngle(curN, curCA, tar);
t = calcDihed(curC, curN, curCA, tar);
find4thPoint(tar, curC, curN, curCA, interCA, a, t);
if( calcDist(target, tar) <= targetTol ) break;
}
// return a phi-psi value for this a,t if available
if(fwd) {
RATdata & ratdata = RATdata::fwdinstance(RATdataPath.c_str());
vector<PSO>::iterator bi, ei;
ratdata.range(resn, interCA, a, t, bi, ei);
int rs = ei - bi;
assert(rs >= 0);
if(rs == 0) { return; }
int incr = (int) floor ( (0.+rs) * ran01() );
bi += incr;
phi = bi->r; psi = bi->a; omega = bi->t;
} else {
RATdata & ratdata = RATdata::bwdinstance(RATdataPath.c_str());
vector<PSO>::iterator bi, ei;
ratdata.range(resn, interCA, a, t, bi, ei);
int rs = ei - bi;
assert(rs >= 0);
if(rs == 0) { return; }
int incr = (int) floor ( (0.+rs) * ran01() );
bi += incr;
psi = bi->r; phi = bi->a; omega = bi->t;
}
//cout << phi <<" "<< psi <<" "<< omega << " shd take u from ("<< curCA[0]<<" "<< curCA[1]<<" "<< curCA[2] <<" ";
//vector<float> exppt(3,0);
//find4thPoint(exppt, curC, curN, curCA, interCA, a, t);
//cout << ") to ("<< exppt[0]<<" "<< exppt[1]<<" "<< exppt[2] <<") " << endl;;
}
// Driver program to test above function
int main()
{
printf("%f \n", calcAngle(11, 59));
printf("%f \n", calcAngle(3, 30));
return 0;
}
void PatternMatching::calcMarkersAngle (Marker *center, std::vector<Marker *> &markers) {
for (unsigned int i=0; i < markers.size(); ++i) {
markers[i]->angle = calcAngle(center->center, markers[i]->center);
}
}
//--------------------------------------------------------------
void ofApp::update(){
activePoses.clear();
bodies.clear();
kinect.update();
unsigned short * depthPixels = this->kinect.getDepthSource()->getPixels();
HRESULT hr = m_pCoordinateMapper->MapColorFrameToDepthSpace(
512 * 424,
(const UINT16*)depthPixels,
1920 * 1080,
m_pDepthCoordinates);
// 1. create tracked bodies/joints array
// THERE IS A MAXIMUM OF 6 BODIES TRACKED BY KINECT
for (int i = 0; i<6; i++){
// IF THE BODY IS BEING TRACKED...
if (this->kinect.getBodySource()->getBodies()[i].tracked){
auto b = this->kinect.getBodySource()->getBodies()[i];
map<JointType, ofxKFW2::Data::Joint>::iterator it;
map<int, ofxKFW2::Data::Joint> jointsData;
if (b.joints.find(JointType_SpineBase) != b.joints.end()) {
if (b.joints.find(JointType_SpineBase)->second.getPosition().z <= maxDistance) {
// ITERATE THROUGH ALL JOINTS IN THE TRACKED BODY...
for (it = b.joints.begin(); it != b.joints.end(); ++it) {
jointsData.insert( pair<int, ofxKFW2::Data::Joint>(it->first, it->second));
}
bodies.push_back(jointsData);
}
}
}
}
// 2. calculate angles
for (vector< map<int, ofxKFW2::Data::Joint> >::iterator i = bodies.begin(); i != bodies.end(); i++) {
int index = i - bodies.begin();
map<string, float> jointAngles;
for (map<string, CalcParams>::iterator paramsIterator = jointCalcParams.begin(); paramsIterator != jointCalcParams.end(); paramsIterator++) {
map<int, ofxKFW2::Data::Joint> b = *i;
auto j1 = b.find(paramsIterator->second.j[0]);
auto j2 = b.find(paramsIterator->second.j[1]);
auto j3 = b.find(paramsIterator->second.j[2]);
if ( checkTracking(j1, j2, j3, b) ) {
float angle = calcAngle( j1, j2, j3 );
jointAngles.insert( pair<string, float>(paramsIterator->first, angle) );
}
}
jointAngleArray.push_back(jointAngles);
// 3. check for poses
// check for angles matching poses
int activePose = -1;
for (map<int, Pose>::iterator j = poses.begin(); j != poses.end(); j++) {
Pose pose = j->second;
float elTar = pose.ElbowLeft;
float erTar = pose.ElbowRight;
float zslTar = pose.zShoulderLeft;
float zsrTar = pose.zShoulderRight;
// get angles or if not found to -1
float elA = (jointAngles.find("elbowLeft") != jointAngles.end()) ? jointAngles.find("elbowLeft")->second : -1.0;
float erA = (jointAngles.find("elbowRight") != jointAngles.end()) ? jointAngles.find("elbowRight")->second : -1.0;
float zslA = (jointAngles.find("zShoulderLeft") != jointAngles.end()) ? jointAngles.find("zShoulderLeft")->second : -1.0;
float zsrA = (jointAngles.find("zShoulderRight") != jointAngles.end()) ? jointAngles.find("zShoulderRight")->second : -1.0;
float elDif = abs( elTar - elA );
float erDif = abs( erTar - erA );
float zslDif = abs( zslTar - zslA );
float zsrDif = abs( zsrTar - zsrA );
// tolerance value
if ( elDif < tol && elA != -1.0 && erDif < tol && erA != -1.0 &&
zslDif < tol && zslA != -1.0 && zsrDif < 10.0 && zsrA != -1.0 ) {
// sendOscMessage(97 + i->first);
// cout << "pose " << j->first << endl;
activePose = j->first;
} else {
// activePoses.push_back(-1.0);
// cout << "pose -1" << endl;
}
}
activePoses.push_back(activePose);
}
scanLineX = ofGetFrameNum() % ofGetWidth();
for (int i = 0; i < bodies.size(); i++) {
if (bodies[i].find(JointType_SpineBase) != bodies[i].end() ) {
ofVec2f pos = bodies[i].find(JointType_SpineBase)->second.getProjected(m_pCoordinateMapper);
if (pos.x > scanLineX && pos.x < scanLineX + 1 ) {
cout << "hitting body " << pos.x << endl;
// cout << "active pose " << activePoses[i] << endl;
if ( activePoses[i] > -1 ) {
cout << "triggering: " << 97 + activePoses[i] << endl;
sendOscMessage(97 + activePoses[i]);
} else {
// play 'fallback sound'
}
}
}
}
// mesh = kinect.getDepthSource()->getMesh(
// false,
// ofxKinectForWindows2::Source::Depth::PointCloudOptions::TextureCoordinates::ColorCamera);
}
int main(void){
Led0 led0;
Sw0 sw0;sw0.setupDigitalIn();
Sw1 sw1;sw1.setupDigitalIn();
Sw2 sw2;sw2.setupDigitalIn();
Sw3 sw3;sw3.setupDigitalIn();
Blink blink0(led0);blink0.setup();blink0.time(200);
Serial1 serial1; serial1.setup(115200);
Serial0 serial0;
Servo servo(serial0);servo.setup();
int data=0;
int flag=0;
float deg=0.00;
long long int time=0;
long long int timer=0;
long long int servotime=0;
int i=0;
while(1){
blink0.cycle();
if(millis()-time>=10){
time=millis();
/*if(millis()-time>=600){
timer=millis();
switch(flag){
case 0:
flag=1;
deg=135;break;
case 1:
flag=2;
deg=0;break;
case 2:
flag=3;
deg=135;break;
case 3:
flag=4;
deg=-135;break;
case 4:
deg=FREE;
flag=0;
i++;
if(i==4) i=0;break;
}
}*/
/*
switch(flag){
case 0:
flag=1;
deg=7500;break;
case 1:
flag=2;
deg=11500;break;
case 2:
flag=0;
deg=3500;break;
}*/
//data=servo.setPos(0,deg);
data=servo.setPos(0,deg);
serial1.printf("%d, %d\n",(int)deg,calcAngle(data));
//data=servo.set_pos(1,deg);
//data=servo.set_pos(2,deg);
//data=servo.set_pos(3,deg);
/*if(!sw0.digitalRead()){
data=servo.set_pos(0,deg);
}else if(!sw1.digitalRead()){
data=servo.set_pos(1,deg);
}else if(!sw2.digitalRead()){
data=servo.set_pos(2,deg);
}else if(!sw3.digitalRead()){
data=servo.set_pos(3,deg);
}*/
}
if(millis()-timer>=600){
timer=millis();
switch(flag){
case 0:
flag=1;
deg=135;break;
case 1:
flag=2;
deg=0;break;
case 2:
flag=3;
deg=135;break;
case 3:
flag=4;
deg=-135;break;
case 4:
deg=FREE;
flag=0;
i++;
if(i==4) i=0;break;
}
}
/*if(millis()-servotime>=30){
if(fabs(deg-deg)!=0){
for(i=0;i<4;i++){
data=servo.set_pos(i,deg);
}
}
}*/
}
}
/*
Returns 1 is edge is locally delaunay
*/
int tesedgeIsLocallyDelaunay( TESShalfEdge *e )
{
return (calcAngle(e->Lnext->Org, e->Lnext->Lnext->Org, e->Org) +
calcAngle(e->Sym->Lnext->Org, e->Sym->Lnext->Lnext->Org, e->Sym->Org)) < (M_PI + 0.01);
}
/* tries to register current frame onto previous frame.
* Algorithm:
* discards fields with low contrast
* select maxfields fields according to their contrast
* check theses fields for vertical and horizontal transformation
* use minimal difference of all possible positions
* calculate shift as cleaned mean of all remaining fields
* calculate rotation angle of each field in respect to center of fields
* after shift removal
* calculate rotation angle as cleaned mean of all angles
* compensate for possibly off-center rotation
*/
Transform calcTransFields(MotionDetect* md, calcFieldTransFunc fieldfunc,
contrastSubImgFunc contrastfunc) {
Transform* ts = (Transform*) ds_malloc(sizeof(Transform) * md->fieldNum);
Field** fs = (Field**) ds_malloc(sizeof(Field*) * md->fieldNum);
double *angles = (double*) ds_malloc(sizeof(double) * md->fieldNum);
int i, index = 0, num_trans;
Transform t;
#ifdef STABVERBOSE
FILE *file = NULL;
char buffer[32];
ds_snprintf(buffer, sizeof(buffer), "k%04i.dat", md->frameNum);
file = fopen(buffer, "w");
fprintf(file, "# plot \"%s\" w l, \"\" every 2:1:0\n", buffer);
#endif
DSVector goodflds = selectfields(md, contrastfunc);
// use all "good" fields and calculate optimal match to previous frame
#ifdef USE_OMP
#pragma omp parallel for shared(goodflds, md, ts, fs) // does not bring speedup
#endif
for(index=0; index < ds_vector_size(&goodflds); index++){
int i = ((contrast_idx*)ds_vector_get(&goodflds,index))->index;
t = fieldfunc(md, &md->fields[i], i); // e.g. calcFieldTransYUV
#ifdef STABVERBOSE
fprintf(file, "%i %i\n%f %f %i\n \n\n", md->fields[i].x, md->fields[i].y,
md->fields[i].x + t.x, md->fields[i].y + t.y, t.extra);
#endif
if (t.extra != -1) { // ignore if extra == -1 (unused at the moment)
ts[index] = t;
fs[index] = md->fields + i;
}
}
t = null_transform();
num_trans = ds_vector_size(&goodflds); // amount of transforms we actually have
ds_vector_del(&goodflds);
if (num_trans < 1) {
ds_log_warn(md->modName, "too low contrast! No field remains.\n"
"(no translations are detected in frame %i)", md->frameNum);
return t;
}
int center_x = 0;
int center_y = 0;
// calc center point of all remaining fields
for (i = 0; i < num_trans; i++) {
center_x += fs[i]->x;
center_y += fs[i]->y;
}
center_x /= num_trans;
center_y /= num_trans;
if (md->show) { // draw fields and transforms into frame.
// this has to be done one after another to handle possible overlap
if (md->show > 1) {
for (i = 0; i < num_trans; i++)
drawFieldScanArea(md, fs[i], &ts[i]);
}
for (i = 0; i < num_trans; i++)
drawField(md, fs[i], &ts[i]);
for (i = 0; i < num_trans; i++)
drawFieldTrans(md, fs[i], &ts[i]);
}
/* median over all transforms
t= median_xy_transform(ts, md->field_num);*/
// cleaned mean
t = cleanmean_xy_transform(ts, num_trans);
// substract avg
for (i = 0; i < num_trans; i++) {
ts[i] = sub_transforms(&ts[i], &t);
}
// figure out angle
if (md->fieldNum < 6) {
// the angle calculation is inaccurate for 5 and less fields
t.alpha = 0;
} else {
for (i = 0; i < num_trans; i++) {
angles[i] = calcAngle(md, fs[i], &ts[i], center_x, center_y);
}
double min, max;
t.alpha = -cleanmean(angles, num_trans, &min, &max);
if (max - min > md->maxAngleVariation) {
t.alpha = 0;
ds_log_info(md->modName, "too large variation in angle(%f)\n",
max-min);
}
}
// compensate for off-center rotation
double p_x = (center_x - md->fi.width / 2);
double p_y = (center_y - md->fi.height / 2);
t.x += (cos(t.alpha) - 1) * p_x - sin(t.alpha) * p_y;
t.y += sin(t.alpha) * p_x + (cos(t.alpha) - 1) * p_y;
#ifdef STABVERBOSE
fclose(file);
#endif
return t;
}
void Ball::calcVector()
{
calcAngle();
calcSpeed();
}
//******************************************************************
float fatan2(float y, float x)
{
#if ASSERTS_ENABLED
float Angle = (float)atan2(y,x);
#endif
if(y == 0.f) // the line is horizontal
{
if( x > 0.f) // towards the right
{
// the angle is 0
CheckReturn(0.f);
}
// toward the left
//CheckReturn(PI);
return float(PI);
}
// we now know that y is not 0 check x
if(x == 0.f) // the line is vertical
{
if( y > 0.f)
{
CheckReturn(PI/2.f);
}
CheckReturn(-PI/2.f);
}
// from here on we know that niether x nor y is 0
if( x > 0.f)
{
// we are in quadrant 1 or 4
if (y > 0.f)
{
// we are in quadrant 1
// now figure out which side of the 45 degree line
if(x > y)
{
CheckReturn(calcAngle(x,y));
}
CheckReturn((PI/2.f) - calcAngle(y,x));
}
// we are in quadrant 4
y = -y;
// now figure out which side of the 45 degree line
if( x > y)
{
CheckReturn(-calcAngle(x,y));
}
CheckReturn(-(PI/2.f) + calcAngle(y,x));
}
// we are in quadrant 2 or 3
x = -x; // flip x so we can use it as a positive
if ( y > 0)
{
// we are in quadrant 2
// now figure out which side of the 45 degree line
if ( x > y)
{
CheckReturn(PI - calcAngle(x,y));
}
CheckReturn(PI/2.f + calcAngle(y,x));
}
// we are in quadrant 3
y = -y; // flip y so we can use it as a positve
// now figure out which side of the 45 degree line
if( x > y)
{
CheckReturn(-PI + calcAngle(x,y));
}
CheckReturn(-(PI/2.f) - calcAngle(y,x));
}
SimpleSkeleton fillSkeletonFromDB(int idBeh, ResultSet *resSkeletons){
SimpleSkeleton skel;
// string pos3d=resSkeletons->getString("pos3D");
// int indexHeight = pos3d.find_first_of(',')+1;
// int lastIndexHeight = pos3d.find_last_of(',')-1;
// int lengthHeight= pos3d.length()-indexHeight-1;
skel.idBehaviour=idBeh;
skel.frame=resSkeletons->getInt("frame");
vec3 head = string2Point3D(resSkeletons->getString(3));
vec3 neck = string2Point3D(resSkeletons->getString(4));
vec3 torso = string2Point3D(resSkeletons->getString(5));
vec3 left_shoulder = string2Point3D(resSkeletons->getString(6));
vec3 left_elbow = string2Point3D(resSkeletons->getString(7));
vec3 left_hand = string2Point3D(resSkeletons->getString(8));
vec3 left_knee = string2Point3D(resSkeletons->getString(9));
vec3 left_foot = string2Point3D(resSkeletons->getString(10));
vec3 left_hip =string2Point3D(resSkeletons->getString(11));
vec3 right_shoulder = string2Point3D(resSkeletons->getString(12));
vec3 right_elbow = string2Point3D(resSkeletons->getString(13));
vec3 right_hand = string2Point3D(resSkeletons->getString(14));
vec3 right_knee = string2Point3D(resSkeletons->getString(15));
vec3 right_foot = string2Point3D(resSkeletons->getString(16));
vec3 right_hip = string2Point3D(resSkeletons->getString(17));
vec3 cameraFloor = vec3(0, 0, 0);
vec3 frontFloor = vec3(torso.x, 0, torso.z); //height 0 means in the floor
skel.height= torso.y;
//now we calculate the angles of the joints
skel.angles[0] = calcAngle(neck, torso, torso, frontFloor); //angle torso with the vertical
skel.angles[1] = calcAngle(left_elbow, left_shoulder, left_shoulder, right_shoulder); //angle left elbow-shoulders
skel.angles[2] = calcAngle(left_hand, left_elbow, left_elbow, left_shoulder); //angle left hand-elbow
skel.angles[3] = calcAngle(left_knee, left_hip, left_hip, right_hip); //angle left knee-hips
skel.angles[4] = calcAngle(left_foot, left_knee, left_knee, left_hip); //angle left ankle-knee
skel.angles[5] = calcAngle(right_elbow, right_shoulder, right_shoulder, left_shoulder); //angle right elbow-shoulders
skel.angles[6] = calcAngle(right_hand, right_elbow, right_elbow, right_shoulder); //angle right hand-elbow
skel.angles[7] = calcAngle(right_knee, right_hip, right_hip, left_hip); //angle right knee-hips
skel.angles[8] = calcAngle(right_foot, right_knee, right_knee, right_hip); //angle right ankle-knee
skel.angles[9] = calcAngle(left_shoulder, right_shoulder, frontFloor, cameraFloor); //angle of rotation of the shoulders
skel.angles[10] = calcAngle(left_hip, right_hip, frontFloor, cameraFloor); //angle of rotation of the hip
// skel.angles[0]=resSkeletons->getDouble("ang0");
// skel.angles[1]=resSkeletons->getDouble("ang1");
// skel.angles[2]=resSkeletons->getDouble("ang2");
// skel.angles[3]=resSkeletons->getDouble("ang3");
// skel.angles[4]=resSkeletons->getDouble("ang4");
// skel.angles[5]=resSkeletons->getDouble("ang5");
// skel.angles[6]=resSkeletons->getDouble("ang6");
// skel.angles[7]=resSkeletons->getDouble("ang7");
// skel.angles[8]=resSkeletons->getDouble("ang8");
// skel.angles[9]=resSkeletons->getDouble("ang9");
// skel.angles[10]=resSkeletons->getDouble("ang10");
return skel;
}
void Spectator::Move::update(float dt)
{
updateAnimation( dt );
// leave blending phase
if( _actionTime < _blendTime ) return;
// determine distance to desired position
Vector3f distance = _pos - _clump->getFrame()->getPos();
if( distance.length() < movePrecision )
{
_endOfAction = true;
return;
}
// calculate motion velocity
float vel;
if( _fast )
{
if( _actionTime - _blendTime < FRAMETIME(281)-FRAMETIME(253) )
{
vel = runVelocity * _actionTime/(FRAMETIME(281)-FRAMETIME(253));
}
else
{
vel = runVelocity;
}
}
else
{
if( _actionTime - _blendTime < FRAMETIME(84)-FRAMETIME(60) )
{
vel = walkVelocity * _actionTime / (FRAMETIME(84)-FRAMETIME(60));
}
else
{
vel = walkVelocity;
}
}
// spectator properties
float width = 25;
float height = 180;
// retrieve current clump position, and raise to the height of spectator
Vector3f pos = _clump->getFrame()->getPos();
pos += Vector3f( 0,1,0 ) * height;
// direction vector
Vector3f dir = _clump->getFrame()->getAt() * vel * dt;
dir += Vector3f( 0,-900,0 ) * dt;
// move it along direction of clump
pos = _enclosure->move( pos, dir, width, height );
// lower position to the ground
pos -= Vector3f( 0,1,0 ) * height;
// determine actual motion distance at this act
Vector3f AMD = pos - _clump->getFrame()->getPos();
if( AMD.length() < ( _clump->getFrame()->getAt() * vel * dt * 0.75f ).length() )
{
_endOfAction = true;
}
// setup position for clump
_clump->getFrame()->setPos( pos );
// determine direction to destination point
dir = _pos - pos;
dir[1] = 0.0f; dir.normalize();
// rotate clump if it is needed
float angle = calcAngle( dir, _clump->getFrame()->getAt(), Vector3f( 0,1,0 ) );
if( fabs( angle ) > 1.0f )
{
_clump->getFrame()->rotateRelative( Vector3f(0,1,0), angle );
}
}
ID getRandomDestinationNode(const RoadSystem *roadSystem, const ID from, const ID start, const double maxDistance ) {
vector< pair<ID, ID> > neighborNodes = roadSystem->getNode(start)->getNextNodes(from);
// No more neighbors? Bad luck.
if (neighborNodes.empty())
return start;
// The node that has been traveled through before
ID lastNode = from;
// The node we are currently at
ID currentNode = start;
// Select a random neighbor-node to go to next. Start with the nodes in front
// and increase the angle if none are found
double allowedAngle = 45; // 45° in both directions
double currentAngle = calcAngle(roadSystem->getNode(currentNode)->getPosition() - roadSystem->getNode(lastNode)->getPosition());
for (; allowedAngle <= (180 + 30); allowedAngle += 30) {
size_t rNode = rand() % neighborNodes.size();
size_t neighborI = rNode;
do {
// Calculate the angle to the neighbor node. If it is near our current angle, accept the node
double angle = calcAngle(roadSystem->getNode(neighborNodes[neighborI].first)->getPosition() - roadSystem->getNode(currentNode)->getPosition());
double deltaAngle = angle - currentAngle;
if (deltaAngle < allowedAngle || deltaAngle > 360 - allowedAngle) {
// Exited the loop per break => Found a node
lastNode = currentNode;
currentNode = neighborNodes[neighborI].first;
allowedAngle = 360; // break angle-loop
break; // node-loop
}
neighborI = (neighborI + 1) % neighborNodes.size();
} while (neighborI != rNode);
}
if (currentNode == start) {
// Nowhere to go...
// Should not happen since there IS a possible neighbor node that should be used
return start;
}
// === After this point, the "start" node means the node that has been found === //
// Run through the road system until there is no further street that leads away from start or the distance is reached
const Vec2f startPosition = roadSystem->getNode(start)->getPosition();
allowedAngle = 45; // now constant
double currentDistance = calcDistance(startPosition, roadSystem->getNode(currentNode)->getPosition());
while (currentDistance < maxDistance) {
// Get the neighbors of the current node
neighborNodes = roadSystem->getNode(currentNode)->getNextNodes(lastNode);
if (neighborNodes.empty()) {
// Nowhere to go? Okay, work done.
return currentNode;
}
Vec2f currentPosition = roadSystem->getNode(currentNode)->getPosition();
// Search for a matching node
size_t rNode = rand() % neighborNodes.size();
size_t neighborI = rNode;
double distance;
bool found = false;
do {
// Calculate the angle to the neighbor node
Vec2f neighborPosition = roadSystem->getNode(neighborNodes[neighborI].first)->getPosition();
double deltaAngle = calcAngle(neighborPosition - currentPosition) - currentAngle;
if (deltaAngle > allowedAngle && deltaAngle < 360 - allowedAngle) {
// Angle does not match, look at next node
neighborI = (neighborI + 1) % neighborNodes.size();
continue;
}
distance = calcDistance(neighborPosition, startPosition);
if (distance > currentDistance) {
found = true;
break;
}
neighborI = (neighborI + 1) % neighborNodes.size();
} while (neighborI != rNode);
if (!found) {
// No further node found, return the current node
return currentNode;
}
// Reset the last and current node
currentDistance = distance;
lastNode = currentNode;
currentNode = neighborNodes[neighborI].first;
}
// Distance reached, return current node
return currentNode;
}
