int main(int argc, char *argv[]){ //check argc if(argc!=4){ printf("Please enter the name of the input data file, the total time, and delta t in s\n"); } double totalTime = atof(argv[2]); double deltaT = atof(argv[3]); body gravity[MAX_ENTRIES]; int i=0, count=0, k=0; double cmx=0., cmy=0., cmz=0., j=0; FILE *outp; outp = fopen("location.dat", "w"); count=readData(argv[1], gravity); centerOfMass(gravity, count, &cmx, &cmy, &cmz); printf("The center of mass of all of the objects is: (%lf, %lf, %lf)\n", cmx, cmy, cmz); gravForce(gravity, count); for(i=0; i<count; i++){ printf("Body %3d Force = (%8.2lg, %8.2lg, %8.2lg)\n", i+1, gravity[i].f_vec[0], gravity[i].f_vec[1], gravity[i].f_vec[2]); } for(j=0; j<totalTime; j+=deltaT){ gravForce(gravity, count); evolve(gravity, count, deltaT); for(k=0; k<count; k++){ fprintf(outp, "%lg %lg %lg\n", gravity[k].s_vec[0], gravity[k].s_vec[1], gravity[k].s_vec[2]); } fprintf(outp, "\n\n"); } fclose(outp); return 0; }
int main(int argc, char *argv[]){ //check argc if(argc!=2){ printf("Please enter the name of the input data file\n"); } body gravity[MAX_ENTRIES]; int i=0, count=0; double cmx=0., cmy=0., cmz=0.; count=readData(argv[1], gravity); centerOfMass(gravity, count, &cmx, &cmy, &cmz); printf("The center of mass of all of the objects is: (%lf, %lf, %lf)\n", cmx, cmy, cmz); gravForce(gravity, count); for(i=0; i<count; i++){ printf("Body %3d Force = (%8.2lg, %8.2lg, %8.2lg)\n", i+1, gravity[i].f_vec[0], gravity[i].f_vec[1], gravity[i].f_vec[2]); } return 0; }
void HoverVehicle::updateForces(F32 /*dt*/) { PROFILE_SCOPE( HoverVehicle_UpdateForces ); Point3F gravForce(0, 0, sHoverVehicleGravity * mRigid.mass * mGravityMod); MatrixF currTransform; mRigid.getTransform(&currTransform); mRigid.atRest = false; mThrustLevel = (mForwardThrust * mDataBlock->mainThrustForce + mReverseThrust * mDataBlock->reverseThrustForce + mLeftThrust * mDataBlock->strafeThrustForce + mRightThrust * mDataBlock->strafeThrustForce); Point3F thrustForce = ((Point3F( 0, 1, 0) * (mForwardThrust * mDataBlock->mainThrustForce)) + (Point3F( 0, -1, 0) * (mReverseThrust * mDataBlock->reverseThrustForce)) + (Point3F(-1, 0, 0) * (mLeftThrust * mDataBlock->strafeThrustForce)) + (Point3F( 1, 0, 0) * (mRightThrust * mDataBlock->strafeThrustForce))); currTransform.mulV(thrustForce); if (mJetting) thrustForce *= mDataBlock->turboFactor; Point3F torque(0, 0, 0); Point3F force(0, 0, 0); Point3F vel = mRigid.linVelocity; F32 baseStabLen = getBaseStabilizerLength(); Point3F stabExtend(0, 0, -baseStabLen); currTransform.mulV(stabExtend); StabPoint stabPoints[2]; stabPoints[0].osPoint = Point3F((mObjBox.minExtents.x + mObjBox.maxExtents.x) * 0.5, mObjBox.maxExtents.y, (mObjBox.minExtents.z + mObjBox.maxExtents.z) * 0.5); stabPoints[1].osPoint = Point3F((mObjBox.minExtents.x + mObjBox.maxExtents.x) * 0.5, mObjBox.minExtents.y, (mObjBox.minExtents.z + mObjBox.maxExtents.z) * 0.5); U32 j, i; for (i = 0; i < 2; i++) { currTransform.mulP(stabPoints[i].osPoint, &stabPoints[i].wsPoint); stabPoints[i].wsExtension = stabExtend; stabPoints[i].extension = baseStabLen; stabPoints[i].wsVelocity = mRigid.linVelocity; } RayInfo rinfo; mFloating = true; bool reallyFloating = true; F32 compression[2] = { 0.0f, 0.0f }; F32 normalMod[2] = { 0.0f, 0.0f }; bool normalSet[2] = { false, false }; Point3F normal[2]; for (j = 0; j < 2; j++) { if (getContainer()->castRay(stabPoints[j].wsPoint, stabPoints[j].wsPoint + stabPoints[j].wsExtension * 2.0, TerrainObjectType | WaterObjectType, &rinfo)) { reallyFloating = false; if (rinfo.t <= 0.5) { // Ok, stab is in contact with the ground, let's calc the forces... compression[j] = (1.0 - (rinfo.t * 2.0)) * baseStabLen; } normalSet[j] = true; normalMod[j] = rinfo.t < 0.5 ? 1.0 : (1.0 - ((rinfo.t - 0.5) * 2.0)); normal[j] = rinfo.normal; } if ( pointInWater( stabPoints[j].wsPoint ) ) compression[j] = baseStabLen; } for (j = 0; j < 2; j++) { if (compression[j] != 0.0) { mFloating = false; // Spring force and damping Point3F springForce = -stabPoints[j].wsExtension; springForce.normalize(); springForce *= compression[j] * mDataBlock->stabSpringConstant; Point3F springDamping = -stabPoints[j].wsExtension; springDamping.normalize(); springDamping *= -getMin(mDot(springDamping, stabPoints[j].wsVelocity), 0.7f) * mDataBlock->stabDampingConstant; force += springForce + springDamping; } } // Gravity if (reallyFloating == false) force += gravForce; else force += gravForce * mDataBlock->floatingGravMag; // Braking F32 vellen = mRigid.linVelocity.len(); if (mThrottle == 0.0f && mLeftThrust == 0.0f && mRightThrust == 0.0f && vellen != 0.0f && vellen < mDataBlock->brakingActivationSpeed) { Point3F dir = mRigid.linVelocity; dir.normalize(); dir.neg(); force += dir * mDataBlock->brakingForce; } // Gyro Drag torque = -mRigid.angMomentum * mDataBlock->gyroDrag; // Move to proper normal Point3F sn, r; currTransform.getColumn(2, &sn); if (normalSet[0] || normalSet[1]) { if (normalSet[0] && normalSet[1]) { F32 dot = mDot(normal[0], normal[1]); if (dot > 0.999) { // Just pick the first normal. They're too close to call if ((sn - normal[0]).lenSquared() > 0.00001) { mCross(sn, normal[0], &r); torque += r * mDataBlock->normalForce * normalMod[0]; } } else { Point3F rotAxis; mCross(normal[0], normal[1], &rotAxis); rotAxis.normalize(); F32 angle = mAcos(dot) * (normalMod[0] / (normalMod[0] + normalMod[1])); AngAxisF aa(rotAxis, angle); QuatF q(aa); MatrixF tempMat(true); q.setMatrix(&tempMat); Point3F newNormal; tempMat.mulV(normal[1], &newNormal); if ((sn - newNormal).lenSquared() > 0.00001) { mCross(sn, newNormal, &r); torque += r * (mDataBlock->normalForce * ((normalMod[0] + normalMod[1]) * 0.5)); } } } else { Point3F useNormal; F32 useMod; if (normalSet[0]) { useNormal = normal[0]; useMod = normalMod[0]; } else { useNormal = normal[1]; useMod = normalMod[1]; } if ((sn - useNormal).lenSquared() > 0.00001) { mCross(sn, useNormal, &r); torque += r * mDataBlock->normalForce * useMod; } } } else { if ((sn - Point3F(0, 0, 1)).lenSquared() > 0.00001) { mCross(sn, Point3F(0, 0, 1), &r); torque += r * mDataBlock->restorativeForce; } } Point3F sn2; currTransform.getColumn(0, &sn); currTransform.getColumn(1, &sn2); mCross(sn, sn2, &r); r.normalize(); torque -= r * (mSteering.x * mDataBlock->steeringForce); currTransform.getColumn(0, &sn); currTransform.getColumn(2, &sn2); mCross(sn, sn2, &r); r.normalize(); torque -= r * (mSteering.x * mDataBlock->rollForce); currTransform.getColumn(1, &sn); currTransform.getColumn(2, &sn2); mCross(sn, sn2, &r); r.normalize(); torque -= r * (mSteering.y * mDataBlock->pitchForce); // Apply drag Point3F vDrag = mRigid.linVelocity; if (!mFloating) { vDrag.convolve(Point3F(1, 1, mDataBlock->vertFactor)); } else { vDrag.convolve(Point3F(0.25, 0.25, mDataBlock->vertFactor)); } force -= vDrag * mDataBlock->dragForce; force += mFloating ? thrustForce * mDataBlock->floatingThrustFactor : thrustForce; // Add in physical zone force force += mAppliedForce; // Container buoyancy & drag force += Point3F(0, 0,-mBuoyancy * sHoverVehicleGravity * mRigid.mass * mGravityMod); force -= mRigid.linVelocity * mDrag; torque -= mRigid.angMomentum * mDrag; mRigid.force = force; mRigid.torque = torque; }