void ball::update(ofVec3f hand, ofVec3f prev_hand, ofVec3f handVel){
//Trigger the average tracking
//80 is the threshold!
if (handVel.length() > 80 && hand.z < prev_hand.z && movementCount == 0) {
currentFood = floor((ofRandom(7)));
isTracking = true;
cout << "threw!" << endl;
}
//else if (handVel.length() < 10) {
else if (movementCount > 60) {
//Throws!
//isTracking = false;
//vel /= movementCount;
movementCount = 0;
pos = ofVec3f (2000, 2000, 0);
vel.set (0, 0, 0);
}
//Tracks
if(isTracking){
pos = hand;
vel = (handVel/3);
//movementCount ++;
movementCount = 1;
isTracking = false;
}else if (movementCount > 0){
pos += vel;
//movementCount = 1;
movementCount++;
}
}
// Make a rotation Quat which will rotate vec1 to vec2
// Generally take adot product to get the angle between these
// and then use a cross product to get the rotation axis
// Watch out for the two special cases of when the vectors
// are co-incident or opposite in direction.
void ofQuaternion::makeRotate_original( const ofVec3f& from, const ofVec3f& to ) {
const float epsilon = 0.0000001f;
float length1 = from.length();
float length2 = to.length();
// dot product vec1*vec2
float cosangle = from.dot(to) / (length1 * length2);
if ( fabs(cosangle - 1) < epsilon ) {
//osg::notify(osg::INFO)<<"*** Quat::makeRotate(from,to) with near co-linear vectors, epsilon= "<<fabs(cosangle-1)<<std::endl;
// cosangle is close to 1, so the vectors are close to being coincident
// Need to generate an angle of zero with any vector we like
// We'll choose (1,0,0)
makeRotate( 0.0, 0.0, 0.0, 1.0 );
} else
if ( fabs(cosangle + 1.0) < epsilon ) {
// vectors are close to being opposite, so will need to find a
// vector orthongonal to from to rotate about.
ofVec3f tmp;
if (fabs(from.x) < fabs(from.y))
if (fabs(from.x) < fabs(from.z)) tmp.set(1.0, 0.0, 0.0); // use x axis.
else tmp.set(0.0, 0.0, 1.0);
else if (fabs(from.y) < fabs(from.z)) tmp.set(0.0, 1.0, 0.0);
else tmp.set(0.0, 0.0, 1.0);
ofVec3f fromd(from.x, from.y, from.z);
// find orthogonal axis.
ofVec3f axis(fromd.getCrossed(tmp));
axis.normalize();
_v[0] = axis[0]; // sin of half angle of PI is 1.0.
_v[1] = axis[1]; // sin of half angle of PI is 1.0.
_v[2] = axis[2]; // sin of half angle of PI is 1.0.
_v[3] = 0; // cos of half angle of PI is zero.
} else {
// This is the usual situation - take a cross-product of vec1 and vec2
// and that is the axis around which to rotate.
ofVec3f axis(from.getCrossed(to));
float angle = acos( cosangle );
makeRotate( angle, axis );
}
}
void AbstractPhysicsBehavior::drawForceArrow(ofVec3f position,
ofVec3f force) {
ofDrawArrow(position,
position + force * 20.0f,
ofClamp(force.length() * 2.0f,
0,
6.0f));
}
//----------
ofVec2f MovingHead::getPanTiltForTargetInObjectSpace(const ofVec3f & objectSpacePoint, float tiltOffset) {
auto pan = atan2(objectSpacePoint.z, objectSpacePoint.x) * RAD_TO_DEG - 90.0f;
if (pan < -180.0f) {
pan += 360.0f;
}
auto tilt = acos(-objectSpacePoint.y / objectSpacePoint.length()) * RAD_TO_DEG; // will always produce positive tilt
return ofVec2f(pan, tilt + tiltOffset);
}
void CorrelateXYZtoXY::push(ofVec3f &xyz, ofVec2f &xy) {
if (xyz.length() < 0.1)
return;
++nPoints;
this->xyz.push_back(toCv(xyz));
this->xy.push_back(toCv(xy));
xyzPreview.addVertex(xyz);
}
//--------------------- AccumulateForce ----------------------------------
//
// This function calculates how much of its max steering force the
// vehicle has left to apply and then applies that amount of the
// force to add.
//------------------------------------------------------------------------
bool SteeringBehaviors::AccumulateForce(ofVec3f &RunningTot, ofVec3f ForceToAdd)
{
float MagnitudeSoFar = RunningTot.length();
double MagnitudeRemaining = m_Vehicle->MaxForce() - MagnitudeSoFar;
if (MagnitudeRemaining <= 0.0) return false;
float MagnitudeToAdd = ForceToAdd.length();
//if the magnitude of the sum of ForceToAdd and the running total
//does not exceed the maximum force available to this vehicle, just
//add together. Otherwise add as much of the ForceToAdd vector is
//possible without going over the max.
if (MagnitudeToAdd < MagnitudeRemaining)
{
RunningTot += ForceToAdd;
}
else
{
RunningTot += (ForceToAdd.getNormalized() * MagnitudeRemaining);
}
return true;
}
// Renders a vector object 'v' as an arrow and a location 'x,y'
void FlowField2D::drawVector(ofVec3f v, float x, float y, float scale) {
ofPushMatrix();
float arrowsize = 4;
// Translate to location to render vector
ofTranslate(x, y);
ofSetColor(100);
// Call vector heading function to get direction (note that pointing up is a heading of 0) and rotate
float rotate = atan2(v.y, v.x);
ofRotate(ofRadToDeg(rotate), 0, 0, 1);
// Calculate length of vector & scale it to be bigger or smaller if necessary
float len = v.length() * scale;
// Draw three lines to make an arrow (draw pointing up since we've rotate to the proper direction)
ofLine(0, 0, len, 0);
ofLine(len, 0, len - arrowsize, +arrowsize / 2);
ofLine(len, 0, len - arrowsize, -arrowsize / 2);
ofPopMatrix();
}
bool FitPlaneToPoints(ofxRay::Plane & plane, const vector<ofVec4f> &Points, ofVec3f &Basis1, ofVec3f &Basis2, float &NormalEigenvalue, float &ResidualError)
{
ofVec3f Centroid, Normal;
float ScatterMatrix[3][3];
int Order[3];
float DiagonalMatrix[3];
float OffDiagonalMatrix[3];
// Find centroid
Centroid = ofVec3f();
float TotalWeight = 0.0f;
for (UINT i = 0; i < Points.size(); i++)
{
TotalWeight += Points[i].w;
Centroid += ofVec3f(Points[i].x, Points[i].y, Points[i].z) * Points[i].w;
}
Centroid /= TotalWeight;
// Compute scatter matrix
Find_ScatterMatrix(Points, Centroid, ScatterMatrix, Order);
tred2(ScatterMatrix, DiagonalMatrix, OffDiagonalMatrix);
tqli(DiagonalMatrix, OffDiagonalMatrix, ScatterMatrix);
/*
** Find the smallest eigenvalue first.
*/
float Min = DiagonalMatrix[0];
float Max = DiagonalMatrix[0];
UINT MinIndex = 0;
UINT MiddleIndex = 0;
UINT MaxIndex = 0;
for (UINT i = 1; i < 3; i++)
{
if (DiagonalMatrix[i] < Min)
{
Min = DiagonalMatrix[i];
MinIndex = i;
}
if (DiagonalMatrix[i] > Max)
{
Max = DiagonalMatrix[i];
MaxIndex = i;
}
}
for (UINT i = 0; i < 3; i++)
{
if (MinIndex != i && MaxIndex != i)
{
MiddleIndex = i;
}
}
/*
** The normal of the plane is the smallest eigenvector.
*/
for (UINT i = 0; i < 3; i++)
{
Normal[Order[i]] = ScatterMatrix[i][MinIndex];
Basis1[Order[i]] = ScatterMatrix[i][MiddleIndex];
Basis2[Order[i]] = ScatterMatrix[i][MaxIndex];
}
NormalEigenvalue = abs(DiagonalMatrix[MinIndex]);
Basis1 *= (DiagonalMatrix[MiddleIndex]) / Basis1.length();
Basis2 *= (DiagonalMatrix[MaxIndex]) / Basis2.length();
if (!isValid(Basis1) || !isValid(Basis2) || !isValid(Normal))
{
plane.setCenter(Centroid);
plane.setNormal(ofVec3f(1, 0, 0));
Basis1 = ofVec3f(0, 1, 0);
Basis2 = ofVec3f(0, 0, 1);
}
else
{
plane.setCenter(Centroid);
plane.setNormal(Normal);
}
ResidualError = 0.0f;
for (UINT i = 0; i < Points.size(); i++)
{
ResidualError += plane.getDistanceTo(ofVec3f(Points[i].x, Points[i].y, Points[i].z));
}
ResidualError /= Points.size();
return true;
}
