bool pointInTurretSight(turret_struct* t, vect3D p, int32* angle, int32* d2)
{
if(!t)return false;
const int32* m=t->OBB->transformationMatrix;
const vect3D u=vectDifference(p,vectDivInt(t->OBB->position,4));
const int32 d=magnitude(u);
const int32 v=dotProduct(u,vect(m[2],m[5],m[8]));
if(angle)*angle=dotProduct(u,vect(m[0],m[3],m[6]));
if(d2)*d2=d;
return v>mulf32(cosLerp(TURRET_SIGHTANGLE),d);
}
static void compute_info(RawArray<const Vector<int,4>> bends, RawArray<const T> angles, RawArray<const TV3> X, RawArray<CubicHinges<TV3>::Info> info) {
for (const int b : range(bends.size())) {
auto& I = info[b];
int i0,i1,i2,i3;bends[b].get(i0,i1,i2,i3);
// Use our ordering for x, Garg et al.'s for e and t.
const TV3 x0 = X[i0], x1 = X[i1], x2 = X[i2], x3 = X[i3],
e0 = x2-x1,
e1 = x3-x1,
e2 = x0-x1,
e3 = x3-x2,
e4 = x0-x2;
const T cross01 = magnitude(cross(e0,e1)),
cross02 = magnitude(cross(e0,e2)),
cross03 = cross01,
cross04 = cross02,
dot00 = sqr_magnitude(e0),
dot01 = dot(e0,e1),
dot02 = dot(e0,e2),
dot03 = -dot(e0,e3),
dot04 = -dot(e0,e4),
cot01 = dot01/cross01,
cot02 = dot02/cross02,
cot03 = dot03/cross03,
cot04 = dot04/cross04,
max_cot = max(cot01,cot02,cot03,cot04),
plate = 6/(cross01+cross02),
beta = (cot01+cot03)*(cot02+cot04)/sqrt(dot00),
cos_rest = cos(angles[b]),
sin_rest = sin(angles[b]);
// Degrade gracefully for bad triangles (those with max_cot > 5)
const T scale = plate/sqr(max(1,max_cot/5));
I.base = scale*dot00*(1-cos_rest);
I.dot = scale*cos_rest;
I.det = scale*beta*sin_rest;
I.c.set(-cot02-cot04,cot03+cot04,cot01+cot02,-cot01-cot03); // Use our ordering for vertices
}
}
void glfwEditGetCursorPos(GLFWwindow *window, double xpos, double ypos) {
//printf("%f %f\n", xpos, g_height - ypos);
lastCurPos = glm::vec2(xpos, g_height - ypos - 1);
//If in GUI Selection, disable pitch and yaw
if(eKeysPressed['G'] == 0) {
if(xpos > g_width || xpos < 0 || ypos < 0 || ypos > g_height) {
return;
}
//Get rid of if unneeded
egaze = GetLookAt() - GetEye();
ew = glm::vec3(-1.0 * ew.x, -1.0 * ew.y, -1.0 * ew.z);
ew = glm::normalize(ew);
eu = glm::cross(GetUp(), ew)/magnitude(glm::cross(GetUp(), ew));
eu = glm::normalize(eu);
eendX = xpos;
eendY = g_height-ypos-1;
float diff;
//Calculate change in X
if(estartX < eendX) {
diff = eendX - estartX;
ebeta = incrementYaw((diff * M_PI)/g_width);
}
else if(estartX > eendX){
diff = estartX - eendX;
ebeta = incrementYaw((-diff * M_PI)/g_width);
}
//Calculate change in Y
if(eendY > estartY && ealpha <= 0.98) {
diff = eendY - estartY;
ealpha = incrementPitch((diff * M_PI)/g_width);
}
else if(eendY < estartY && ealpha >= -0.98) {
diff = estartY - eendY;
ealpha = incrementPitch(-(diff * M_PI)/g_width);
}
estartX = g_width/2.0;// = endX;
estartY = g_height/2.0-1;// endY;
}
}
Quaternion Quaternion::GetConjugate() const
{
Quaternion result = *this;
float mag = magnitude(*this);
result.v = -v;
if (mag != 0)
{
result *= (1 / mag);
}
return result;
}
Quaternion & Quaternion::normalize() {
float mplier = magnitude();
if(INFINTESSIMAL(mplier)) {
degenerate = true;
return *this;
}
mplier = 1.0f / mplier;
time *= mplier;
space *= mplier;
return *this;
}
Vector2D computeVelocity(Vector2D start, Vector2D end, COIN *cToBeHit,COIN *cHitBy,Vector2D velRequired)
{
Vector2D fVel;
double fVelMag = 0;
Vector2D dir;
dir = subtract(&end,&start);
double mag = magnitude(dir.X,dir.Y);
dir.X = dir.X / mag;
dir.Y = dir.Y / mag;
Vector2D vReqDir;
if(cToBeHit == NULL)
{
fVelMag = sqrt(2 * friction * mag * 1.6f );
fVel.X = dir.X * fVelMag;
fVel.Y = dir.Y * fVelMag;
}else
{
double reqVel = magnitude(velRequired.X,velRequired.Y);
vReqDir.X = velRequired.X / reqVel;
vReqDir.Y = velRequired.Y / reqVel;
double xpVel = (magnitude(velRequired.X,velRequired.Y) * (cToBeHit->mass + cHitBy->mass))/(cHitBy->mass * (1.0f+eCS));
double cosT = dotProduct(&dir,&vReqDir);
if(cosT < 0) cosT = -cosT;
double iniVelReq = sqrt((xpVel / cosT) * (xpVel / cosT) + 2 * friction * mag * 1.6f);
fVel.X = iniVelReq * dir.X;
fVel.Y = iniVelReq * dir.Y;
if(iniVelReq > maxPower)
{
fVel.X = maxPower * dir.X;
fVel.Y = maxPower * dir.Y;
}
}
return fVel;
}
void dynExprField::apply(
MDataBlock &block,
int receptorSize,
const MDoubleArray &magnitudeArray,
const MDoubleArray &magnitudeOwnerArray,
const MVectorArray &directionArray,
const MVectorArray &directionOwnerArray,
MVectorArray &outputForce
)
//
// Compute output force for each particle. If there exists the
// corresponding per particle attribute, use the data passed from
// particle shape (stored in magnitudeArray and directionArray).
// Otherwise, use the attribute value from the field.
//
{
// get the default values
MVector defaultDir = direction(block);
double defaultMag = magnitude(block);
int magArraySize = magnitudeArray.length();
int dirArraySize = directionArray.length();
int magOwnerArraySize = magnitudeOwnerArray.length();
int dirOwnerArraySize = directionOwnerArray.length();
int numOfOwner = magOwnerArraySize;
if( dirOwnerArraySize > numOfOwner )
numOfOwner = dirOwnerArraySize;
double magnitude = defaultMag;
MVector direction = defaultDir;
for (int ptIndex = 0; ptIndex < receptorSize; ptIndex ++ ) {
if(receptorSize == magArraySize)
magnitude = magnitudeArray[ptIndex];
if(receptorSize == dirArraySize)
direction = directionArray[ptIndex];
if( numOfOwner > 0) {
for( int nthOwner = 0; nthOwner < numOfOwner; nthOwner++ ) {
if(magOwnerArraySize == numOfOwner)
magnitude = magnitudeOwnerArray[nthOwner];
if(dirOwnerArraySize == numOfOwner)
direction = directionOwnerArray[nthOwner];
outputForce.append( direction * magnitude );
}
} else {
outputForce.append( direction * magnitude );
}
}
}
void calcforces(int nprt,int ndim,double *coord,double *force)
{
/* calc forces */
int i,j,k;
double d,rij[3];
for(i=0; i<nprt; i++) {
for(j=0; j<nprt; j++) {
if( j==i ) continue;
for(k=0; k<ndim; k++) {
dist(ndim,&coord[i*ndim],&coord[j*ndim],rij);
d=magnitude(ndim,rij);
force[i*ndim+k] += -rij[k]*dV(d)/d;
}
}
}
}
void normalize(void)
{
float mag=magnitude();
#ifdef assertMyth
assertMyth("NewtVector::normalize found magnitude of zero",
mag > 0.0f);
#endif
if (mag > 0.0f) // can't normalize a vector of magnitude 0.
{
i = i / mag;
j = j / mag;
k = k / mag;
}
}
double calcpotent(int nprt,int ndim,double *coord)
{
/* calc potential energy */
int i,j;
double d,rij[3];
double pot=0.0;
for(i=0; i<nprt; i++) {
for(j=0; j<nprt; j++) {
if( j==i ) continue;
dist(ndim,&coord[i*ndim],&coord[j*ndim],rij);
d=magnitude(ndim,rij);
pot+=1.0/2.0*V(d);
}
}
return pot;
}
f_sqrt()
{
struct value a;
register double mag, ang;
(void) pop(&a);
mag = sqrt(magnitude(&a));
if (imag(&a) == 0.0 && real(&a) < 0.0)
push( complex(&a,0.0,mag) );
else
{
if ( (ang = angle(&a)) < 0.0)
ang += 2*Pi;
ang /= 2;
push( complex(&a,mag*cos(ang), mag*sin(ang)) );
}
}
int main(int argc, char *argv[]) {
TEST_NAME();
auto q1 = Quaternion(1,2,3,0);
auto q2 = Quaternion(1,1,1,1);
auto q3 = q1 + q2;
TEST_EQUAL(q3, Quaternion(2,3,4,1), "quaternion addition");
std::cout << "q1: " << q1 << std::endl;
std::cout << "q2: " << q2 << std::endl;
std::cout << "q1 * q2 = " << q1 * q2 << std::endl;
TEST_EQUAL(q1*q2, 6.0, "scalar multiplication");
std::cout << "q1 x q2 = " << q1 % q2 << std::endl;
TEST_EQUAL(q1%q2, Quaternion(-4,6,2,0), "product multiplication");
TEST_EQUAL(q1.conj(), Quaternion(1,-2,-3,0), "Conjugate");
TEST_EQUAL(q2.magnitude_real(), 2, "Real Magnitude");
TEST_EQUAL(q2.magnitude(), 4, "Magnitude");
auto q4 = Quaternion(5,3,2,2);
std::cout << "q4-unit: " << q4.unit() << std::endl;
std::cout << "q4-inverse: " << q4.inv() << std::endl;
auto q5 = Quaternion(4,5,1,2);
auto q6 = q5 % q1;
q5 %= q1;
TEST_EQUAL(q5, q6, "testing product2");
auto q7 = Quaternion();
q7[0] = 1;
q7[1] = 2;
q7[2] = q7[0] + q7[1];
q7[3] = q7[0] + q7[2];
std::cout << "q7: " << q7 << std::endl;
TEST_EQUAL(q7, Quaternion(1,2,3,4), "testing assignment");
return 0;
}
void gabor_filter(cv::Mat& img,vector<cv::Mat> &featureMaps)
{
//cv::Mat img input character image
const int kernel_size = 7; // should be odd
// variables for gabor filter
double Kmax = PI/2;
double f = sqrt(2.0);
double sigma = 2*PI;
int U = 0;
int V = 0;
int GaborH = kernel_size;
int GaborW = kernel_size;
int UStart = 0, UEnd = 8;
int VStart = 1, VEnd = 2;
// variables for filter2D
cv::Point archor(-1,-1);
int ddepth = CV_32F;//CV_64F
//double delta = 0;
//eight orientation in terms of one frequnecy
for(V = VStart; V < VEnd; V++){
for(U = UStart; U < UEnd; U++){
cv::Mat kernel_re, kernel_im;
cv::Mat dst_re, dst_im, dst_mag;
kernel_re = getMyGabor(GaborW, GaborH, U, V,
Kmax, f, sigma, CV_32F, "real");
kernel_im = getMyGabor(GaborW, GaborH, U, V,
Kmax, f, sigma, CV_32F, "imag");
// flip kernel
// Gabor kernel is symmetric, so do not need flip
filter2D(img, dst_re, ddepth, kernel_re);
filter2D(img, dst_im, ddepth, kernel_im);
dst_mag.create(img.rows, img.cols, CV_32FC1);
magnitude(cv::Mat(dst_re),cv::Mat(dst_im),
dst_mag);
//normalize gabor kernel
cv::normalize(dst_mag, dst_mag, 0, 255, CV_MINMAX);
dst_mag.convertTo(dst_mag,CV_8U);
featureMaps.push_back(dst_mag);
kernel_re.release();
kernel_im.release();
dst_re.release();
dst_im.release();
dst_mag.release();
}
}
}
void applyDFTOperation(Mat &image,Mat &magImage)
{
int row = getOptimalDFTSize(image.rows);
int col = getOptimalDFTSize(image.cols);
Mat image2;
copyMakeBorder(image,image2,0,row-image.rows,0,col-image.cols,BORDER_CONSTANT, Scalar::all(0));
Mat planes[] = {Mat_<float>(image2), Mat::zeros(image2.size(), CV_32F)};
Mat complexI;
merge(planes,2, complexI);
dft(complexI,complexI);
split(complexI,planes);
magnitude(planes[0],planes[1],planes[0]);
magImage = planes[0];
magImage += Scalar::all(1);
log(magImage,magImage);
magImage = magImage(Rect(0,0,magImage.cols & -2,magImage.rows & -2));
int cx = magImage.cols/2;
int cy = magImage.rows/2;
Mat q0(magImage,Rect(0,0,cx,cy)); //左上
Mat q1(magImage,Rect(cx,0,cx,cy)); //右上
Mat q2(magImage,Rect(0,cy,cx,cy)); //左下
Mat q3(magImage,Rect(cx,cy,cx,cy)); //右下
//将左上和右下交换 右上和左下交换
Mat tmp;
q0.copyTo(tmp);
q3.copyTo(q0);
tmp.copyTo(q3);
q1.copyTo(tmp);
q2.copyTo(q1);
tmp.copyTo(q2);
normalize(magImage, magImage, 0, 1, CV_MINMAX);
magImage.convertTo(magImage, CV_8UC1, 255);
}
void
MFCC::process(fvec& in, fvec& out)
{
unsigned int i,k;
if ((in.size() != inSize_) || (out.size() != outSize_))
{
cerr << "Warnging: MFCC::process: inSize_ and input window size do not agree" << endl;
return;
}
hamming_->process(in, windowed);
magfft_->process(windowed, magnitude);
for (i=0; i < inSize_/2; i++)
fmagnitude(i) = magnitude(i);
for (i=0; i< inSize_/2; i++)
fmagnitude(i+ inSize_/2) = fmagnitude(inSize_/2 - i);
float sum =0.0;
// Calculate the filterbank responce
for (i=0; i<totalFilters_; i++)
{
sum = 0.0;
for (k=0; k<fftSize_; k++)
{
sum += (mfccFilterWeights_(i, k) * fmagnitude(k));
}
if (sum != 0.0)
earMagnitude_(i) = log10(sum);
else
earMagnitude_(i) = 0.0;
}
// Take the DCT
for (i=0; i < cepstralCoefs_; i++)
{
sum =0.0;
for (k=0; k < totalFilters_; k++)
{
sum += (mfccDCT_(i,k) * earMagnitude_(k));
}
out(i) = sum;
}
}
int main(int argc, char* argv[]) {
/* Arguments */
if ( argc != 2 ) {
printf("\nUsage: %s <file>\n\n", argv[0]);
exit(0);
}
pgm_t* src = pgm_read(argv[1]);
dmap_t* sobx = sobel_x(src);
dmap_t* soby = sobel_y(src);
dmap_t* sobm = magnitude(src, sobx, soby);
dmap_t* mxx = multiply(src, sobx, sobx);
dmap_t* myy = multiply(src, soby, soby);
dmap_t* mxy = multiply(src, sobx, soby);
dmap_t* mxxf = binomial_filter(mxx);
dmap_t* myyf = binomial_filter(myy);
dmap_t* mxyf = binomial_filter(mxy);
dmap_t* harris1 = harris(src, mxxf, myyf, mxyf, 1);
dmap_t* harris1f = harris(src, mxxf, myyf, mxyf, 1);
dmap_write(sobx, argv[1], "sobel_x");
dmap_write(soby, argv[1], "sobel_y");
dmap_write(sobm, argv[1], "sobel_magnitude");
dmap_write(mxx, argv[1], "xx");
dmap_write(myy, argv[1], "yy");
dmap_write(mxy, argv[1], "xy");
dmap_write(mxxf, argv[1], "xxf");
dmap_write(myyf, argv[1], "yyf");
dmap_write(mxyf, argv[1], "xyf");
dmap_write(harris1, argv[1], "harris1");
dmap_write(harris1f, argv[1], "harris1f");
pgm_free(src);
dmap_free(sobx);
dmap_free(soby);
dmap_free(sobm);
dmap_free(mxx);
dmap_free(myy);
dmap_free(mxy);
dmap_free(harris1);
return 0;
}
/// Normalizes this quaternion in place
void QUAT::normalize()
{
REAL mag = magnitude();
if (mag != (REAL) 0.0)
{
REAL magi = (REAL) 1.0/mag;
x *= magi;
y *= magi;
z *= magi;
w *= magi;
}
else
{
x = y = z = 0;
w = 1;
}
}
boost::shared_ptr< i_property_values const > sat_system< dim >::get_state_property_values() const
{
rtp_stored_property_values* vals = new rtp_stored_property_values();
set_state_prop(vals, Time, m_state.time);
set_state_prop(vals, PosX, m_state.ship.position[0]);
set_state_prop(vals, PosY, m_state.ship.position[1]);
set_state_prop(vals, Angle, m_state.ship.orientation);
set_state_prop(vals, VelX, m_state.ship.lin_velocity[0]);
set_state_prop(vals, VelY, m_state.ship.lin_velocity[1]);
double speed = magnitude(m_state.ship.lin_velocity);
set_state_prop(vals, Speed, speed);
double ang_speed = m_state.ship.ang_velocity; // TODO: For 3D should be magnitude()...
set_state_prop(vals, AngularSpeed, ang_speed);
double ke = 0.5 * (speed * speed + ang_speed * ang_speed); // TODO: m and I
set_state_prop(vals, KE, ke);
return boost::shared_ptr< i_property_values const >(vals);
}
void updateDeltas(NeuralNetwork* net, double* target) {
//TODO: could test for net validity, but do I really need to?
unsigned int layer, size;
if(!net || !target) {
return;
}
layer = net->layers - 1;
size = net->layerSizes[layer];
//delta_l = (activation_l - target)*sigma'(preActivation_l)
subtract(target, net->activations[layer], net->biasDelta[layer - 1], size);
applyOnEach(net->preActivations[layer], net->scratchPaper, net->activationFunctionDerivative, size);
hadamardProduct(net->biasDelta[layer - 1], net->scratchPaper, net->biasDelta[layer - 1], size);
//dC/db_l = delta_l
//TODO: beware of unsigned int underflow
//work backwards for each layer
//do {
#define DEBUG 1
#if DEBUG
puts("########################################");
printf("layer %d, size %d\n", layer, size);
puts("TARGET");
printVector(stdout,target,size);
puts("PREACTIVATIONS");
printVector(stdout,net->preActivations[layer],size);
puts("sigma'(preActivations)");
printVector(stdout,net->scratchPaper,size);
puts("bias delta");
printVector(stdout,net->biasDelta[layer - 1],size);
printf("error: %f\n", magnitude(net->biasDelta[layer - 1], size));
puts("CURRENT STATE");
printNet(stdout, net, 1);
#endif
/*
layer--;
size = net->layerSizes[layer];
//delta_l = (W_l+1^T * delta_l+1) hprod sigma'(preactive_l)
applyOnEach(net->preActivations[layer],net->scratchPaper, net->activationFunctionDerivative, size);
matrixTransposeVectorProduct(net->weights[layer], net->biasDelta[layer],
net->biasDelta[layer - 1], size, net->layerSizes[layer + 1]);
} while(layer > 0);
*/
//dc/db = delta_l
//dc/dw = act_l-1 delta_l
//for the previous layers
//
}
Mat create_spectrum(Mat &complexImg){
Mat planes[2];
split(complexImg, planes);
magnitude(planes[0], planes[1], planes[0]);
Mat magI = (planes[0]).clone();
magI +=Scalar::all(1);
log(magI, magI);
shiftDFT(magI);
normalize(magI, magI, 0, 1, CV_MINMAX);
return magI;
}
int main(){
Complex z;
z.re = 1.0;
z.im = 2.0;
printf("%ld\n", factorial(5));
printf("%lf \n", ipower(3,2));
//print(z.re,z.im);
printf("mag is %lf \n", magnitude(z));
//printf("conjugate is ")
conjugate(&z);
print(z);
return(0);
}
Platform* Player::choose_next_plat()
{
Vector<float,2> input( 0, 0 );
if( Keyboard::key_down('w') )
input.y() += -1;
if( Keyboard::key_down('s') )
input.y() += 1;
if( Keyboard::key_down('a') )
input.x() += -1;
if( Keyboard::key_down('d') )
input.x() += 1;
Platform* next = 0;
if( magnitude(input) > 0.01f )
{
// Input is rotated to match perspective.
Vector<float,2> direction;
float rotate = -World::zRotDeg * 3.14f/180.f;
direction.x( std::cos(rotate)*input.x() - std::sin(rotate)*input.y() );
direction.y( std::sin(rotate)*input.x() + std::cos(rotate)*input.y() );
// Find platform most in the direction the player is facing.
Platform* nextPlat = 0;
float minAngle = 366;
for( size_t i=0; i < plat->adjacents.size(); i++ )
{
Vector<float,2> d = plat->adjacents[i]->s - plat->s;
float angle = angle_between( direction, d );
if( angle < minAngle ) {
minAngle = angle;
nextPlat = plat->adjacents[i];
}
}
if( nextPlat && minAngle < 3.14 / 4 )
next = nextPlat;
else
next = 0;
}
return next;
}
Mat THDUtil::computeOrientation(Mat frame)
{
Mat padded; //expanding input image to optimal size by making boarder
int m = getOptimalDFTSize(frame.rows);
int n = getOptimalDFTSize(frame.cols);
copyMakeBorder(frame, padded, m - frame.rows, 0, n - frame.cols, BORDER_CONSTANT, 0);
Mat planes[] = { Mat_<float>(padded), Mat::zeros(padded.size(), CV_32F) };
Mat complexI;
merge(planes, 2, complexI);
dft(complexI, complexI);
split(complexI, planes); // planes[0] = Re(DFT(I), planes[1] = Im(DFT(I))
magnitude(planes[0], planes[1], planes[0]);// planes[0] = magnitude
Mat magI = planes[0];
magI += Scalar::all(1); // switch to logarithmic scale
log(magI, magI);
// crop the spectrum, if it has an odd number of rows or columns
magI = magI(Rect(0, 0, magI.cols & -2, magI.rows & -2));
// rearrange the quadrants of Fourier image so that the origin is at the image center
int cx = magI.cols / 2;
int cy = magI.rows / 2;
Mat q0(magI, Rect(0, 0, cx, cy)); // Top-Left - Create a ROI per quadrant
Mat q1(magI, Rect(cx, 0, cx, cy)); // Top-Right
Mat q2(magI, Rect(0, cy, cx, cy)); // Bottom-Left
Mat q3(magI, Rect(cx, cy, cx, cy)); // Bottom-Right
Mat tmp; // swap quadrants (Top-Left with Bottom-Right)
q0.copyTo(tmp);
q3.copyTo(q0);
tmp.copyTo(q3);
q1.copyTo(tmp); // swap quadrant (Top-Right with Bottom-Left)
q2.copyTo(q1);
tmp.copyTo(q2);
normalize(magI, magI, 0, 1, CV_MINMAX); // Transform the matrix with float values into a
// viewable image form (float between values 0 and 1).
return magI;
}
int Hook2::handle_damage(SpaceLocation *src, double normal, double direct)
{
STACKTRACE;
if ( src == hooktarget )
return 0;
if ( src->isShip() || (bHitAsteroids && src->isAsteroid())) {
if ( hooktarget ) // if it's already attached to another ship
return 0;
if ( src == ship )
return 0; // if it is the ship itself (doh)
hooktarget = src;
game->play_sound(data->sampleSpecial[1],this);
hookfixdist = magnitude(min_delta(pos, src->pos, map_size));
hookfixorientation = atan(min_delta(pos, src->pos, map_size)) - src->angle;
hookfixangle = angle - src->angle;
collide_flag_anyone = 0;
collide_flag_sameteam = 0;
collide_flag_sameship = 0;
hooklocked = 1; // stop unrolling any further
if (src->isShip()) {
src->ship->turn_rate *= 0.80;
damage(src, 1.0, 0.0);
}
return 0;
} else {
SpaceObject::handle_damage(src, normal, direct);
armour -= normal + direct;
if ( armour <= 0 )
state = 0;
return iround(normal + direct);
}
}
Vector3f & Vector3f::normalize() {
float mplier = magnitude();
if(INFINTESSIMAL(mplier)) {
degenerate = true;
return *this;
}
else
degenerate = false;
mplier = 1.0f / mplier;
i *= mplier;
j *= mplier;
k *= mplier;
return *this;
}
//----------------------------------------------------------------------------
float Vector3::unitize(float fTolerance)
{
float fMagnitude = magnitude();
if (fMagnitude > fTolerance)
{
float fInvMagnitude = 1.0f / fMagnitude;
x *= fInvMagnitude;
y *= fInvMagnitude;
z *= fInvMagnitude;
}
else
{
fMagnitude = 0.0f;
}
return fMagnitude;
}
float distance(Vector2D* p, Trajectory* t) {
Vector2D u,*v,dist;
v = t->direction;
minus_void(p,t->source, &u);
float pp = dot_product(&u,v) / dot_product(v,v);
Vector2D proj;
set(&proj,v->x, v->y);
normalize_void(&proj);
mult_constant_void(&proj,pp);
minus_void(&u,&proj,&dist);
float mag = magnitude(&dist);
return mag;
}
void Hose::calc_em()
{
/*
* rate = dt*mag(v) < spacing ?
* emit every [spacing / (dt*v)] calls
* em = (int) (1 + spacing/dt/mag(v))
* count every call to spray, emit when count == em, restart counter
*/
//em = 0;
//em_count = 0;
float dt = ps->settings->dt;
float magv = magnitude(velocity);
//printf("magv: %f\n", magv);
//em = (int) (1 + spacing/dt/magv/8.);
//why do i have to divide by 4?
em = (int) (1 + spacing/dt/magv/10);
//printf("em: %d\n", em);
}
// 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;
}
bool rocks_in_space::collides(const collision& col_a, const collision& col_b)
{
auto delta = col_a.m_position - col_b.m_position;
if (delta.magnitude() > (col_a.m_radius + col_b.m_radius))
{
return false;
}
auto segment_start = col_a.m_path.m_vertices[col_a.m_path.m_count - 1];
return &col_a.m_path.m_vertices[col_a.m_path.m_count] != std::find_if(&col_a.m_path.m_vertices[0], &col_a.m_path.m_vertices[col_a.m_path.m_count], [&](const auto& v)
{
if (collides(col_b, { { segment_start + col_a.m_position, v + col_a.m_position } }))
{
return true;
}
segment_start += v;
return false;
});
}
