Exemplo n.º 1
0
	DPoint SpringEmbedderGridVariant::ForceModelHachul::computeDisplacement(int j, double boxLength) const
	{
		const double cEps = eps();
		const double cIELEps = m_idealEdgeLength + cEps;

		const NodeInfo &vj = m_vInfo[j];
		int grid_x = vj.m_gridX;
		int grid_y = vj.m_gridY;

		// repulsive forces on node j: F_rep(d) = iel^2 / d
		DPoint force(0,0);
		for(int gi = -1; gi <= 1; gi++) {
			for(int gj = -1; gj <= 1; gj++) {
				for(int u : m_gridCell(grid_x+gi,grid_y+gj)) {

					if(u == j) continue;
					DPoint dist = vj.m_pos - m_vInfo[u].m_pos;
					double d = dist.norm();

					if(d < boxLength) {
						dist /= d * d + cEps;
						force += dist;
					}
				}
			}
		}

		force *= m_idealEdgeLength * m_idealEdgeLength;
		DPoint disp(force);

		// attractive forces on j: F_attr(d) = -d^2 log_2(d/iel) / iel
		DPoint forceRepSub(0,0); // subtract rep. force on adjacent vertices
		force = DPoint(0,0);
		for(int l = vj.m_adjBegin; l != vj.m_adjStop; ++l) {
			int u = m_adjLists[l];

			DPoint dist = vj.m_pos - m_vInfo[u].m_pos;
			double d = dist.norm();

			force -= d * Math::log2((d+cEps) / cIELEps) * dist;
			if(d < boxLength) {
				double f = 1.0 / (d * d + cEps);
				forceRepSub += f * dist;
			}
		}

		force       /= m_idealEdgeLength;
		forceRepSub *= m_idealEdgeLength * m_idealEdgeLength;
		disp += force - forceRepSub;

		return disp;
	}
Exemplo n.º 2
0
// add force and torque to rigid body
void  PhysicsApplyGravityForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;

	NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
//mass*= 0.0f;

	dVector force (dVector (0.0f, 1.0f, 0.0f).Scale (mass * DEMO_GRAVITY));
	NewtonBodySetForce (body, &force.m_x);
}
Exemplo n.º 3
0
void _cdecl Body::standardForceCallback( OgreNewt::Body* me )
{
	//apply a simple gravity force.
	Ogre::Real mass;
	Ogre::Vector3 inertia;

	me->getMassMatrix(mass, inertia);
	Ogre::Vector3 force(0,-9.8,0);
	force *= mass;

	me->addForce( force );

}
Exemplo n.º 4
0
void ForestWindAccumulator::applyImpulse( const VectorF &impulse )
{
   // First build the current force.
   VectorF force( mCurrentDir.x, mCurrentDir.y, 0 );

   // Add in our mass corrected force.
   const F32 mass = mDataBlock->mMass * mScale;
   force += impulse / mass;

   // Set the new direction and force.
   mCurrentDir.set( force.x, force.y );  
   mCurrentStrength += impulse.len() * TickSec;
}
Exemplo n.º 5
0
void TailDrawable::Constraint::solve()
{
    auto constraintVec = m_massA.getPosition() - m_massB.getPosition();
    auto vecLength = Util::Vector::length(constraintVec);

    sf::Vector2f force(0.f ,0.f);
    if (vecLength != 0)
        force += -(constraintVec / vecLength) * (vecLength - constraintLength) * stiffness;

    force += -(m_massA.getVelocity() - m_massB.getVelocity()) * friction;
    m_massA.applyForce(force);
    m_massB.applyForce(-force);
}
void vParticleOperator_WorldRotationForce::Simulate( vParticle *parent )
{
	QAngle cur = MainViewAngles();

	Vector2D force( AngleDiff( cur.y, last.y ), AngleDiff( last.x, cur.x ) );
	force *= GetImpulse( parent ) * vParticle::GetRelativeScale() * str;

	float rainHack = CFrameTimeHelper::GetFrameTime() - (1.0f/60.0f);
	force += force * rainHack * -20.0f;

	last = cur;

	parent->vecVelocity += force;
}
Exemplo n.º 7
0
void PeriIsoCompressor::avgStressIsoStiffness(const Vector3r& cellAreas, Vector3r& stress, Real& isoStiff) {
    Vector3r force(Vector3r::Zero());
    Real stiff=0;
    long n=0;
    FOREACH(const shared_ptr<Contact>& C, *dem->contacts) {
        const auto fp=dynamic_pointer_cast<FrictPhys>(C->phys); // needed for stiffness
        if(!fp) continue;
        force+=(C->geom->node->ori*C->phys->force).array().abs().matrix();
        stiff+=(1/3.)*fp->kn+(2/3.)*fp->kt; // count kn in one direction and ks in the other two
        n++;
    }
    isoStiff= n>0 ? (1./n)*stiff : -1;
    stress=-Vector3r(force[0]/cellAreas[0],force[1]/cellAreas[1],force[2]/cellAreas[2]);
}
Exemplo n.º 8
0
bool Entity::update(float deltaTime) {
  b2Vec2 velocity = _body->GetLinearVelocity();
  b2Vec2 force(0.0f, 0.0f), acceleration(0.0f, 0.0f);

  acceleration.x = _velocity.x - velocity.x;
  acceleration.y = _velocity.y - velocity.y;
  
  force = _body->GetMass() * acceleration;

  //_body->ApplyLinearImpulse(force, _body->GetWorldCenter(), true);
  _body->SetLinearVelocity(Utils::toB2Vec2(_velocity));

  return true;
}
Exemplo n.º 9
0
 Vector3D ChargeSystem::getForce(int c) const
 {
   int n = d_particles.size();
   const PhasePoint& current = d_particles[c];
   Vector3D force( -10. * current.d_v ); // viscosity
   for(int i = 0; i < n; ++i)
   {
     if(i == c)
       continue;
     force += coulombForce(current.d_p, d_particles[i].d_p);
   }
   force -= (force * current.d_p) * current.d_p; // force is acting in tangent space of the unit sphere
   return force;
 }
void ForceRendererTest::common2D() {
    Vector2 force(2.7f, -11.5f);
    Matrix3 m = Implementation::forceRendererTransformation<2>({0.5f, -3.0f}, force);

    /* Translation, right-pointing base vector is the same as force */
    CORRADE_COMPARE(m.translation(), Vector2(0.5f, -3.0f));
    CORRADE_COMPARE(m.right(), force);

    /* All vectors have the same length */
    CORRADE_COMPARE(m.up().length(), force.length());

    /* All vectors are orthogonal */
    CORRADE_COMPARE(Math::dot(m.right(), m.up()), 0.0f);
}
Exemplo n.º 11
0
void MSnstar::addNForce(Mrocket rocket , b2Vec2 position)
{
	float x , y , Fx , Fy , Xx , Xy ,RR;
	x = position.x;
	y = position.y;
	Xx = x - locationX ;
	Xy = y - locationY ; 
	RR = ( Xx )*( Xx )+( Xy )*( Xy ); 
	//RR = pow( RR , 1.5f) ; 
	Fx = Kn * Xx / RR ;
	Fy = Kn * Xy / RR ;
	b2Vec2 force( Fx , Fy );
	rocket.getRocketBody() ->ApplyForce(force,rocket.getRocketBody()->GetWorldCenter()); 
}
Exemplo n.º 12
0
Vector3d Scene::computeSimpleForces(Particle *particle) {
   
    Vector3d force(0, 0, 0);
   
   // compute gravity and drag forces 
   if (gravity == true) {
       force.setCoords(force.X(), force.Y() + particle->mass*GRAVITY, force.Z()); 
   
       force = force + particle->drag*particle->vel*(1/particle->mass);
   }

   return force;

}
Exemplo n.º 13
0
void Particle_Update(ParticleSystem* psystem, f32 dt) {
	u16 n = psystem->n_particles;

	for (u16 i = 0; i < n; i++) {
		Vec3& pos = psystem->positions[i];
		Vec3& vel = psystem->velocities[i];

		Vec3 force(0, -9.81f*0.0005f*dt, 0);
		vel += force;

		pos += vel;
		psystem->ages[i] += dt;
	}
}
Exemplo n.º 14
0
int main(){
    float force(float);
    float v;
    float f = 0;
    printf("Please enter an initail position : ");
    scanf("%f",&f);
    printf("Please enter all the signal and enter y to exit : ");
    while(scanf(" %f",&v)){
        float mov  = force(v);
        f += mov;
    }
    printf("The final position is %f",f);
    
    return 0;
}
Exemplo n.º 15
0
// These two functions calculate the net force and torque
// Caused by internal components of the Spacecraft
Vec2 Spacecraft::calc_net_force()
{
  Vec2 force(0,0);
  // Loop thru each actuator and sum linear effects
  for (uint i=0; i<actuators_.size(); i++)
    force = force + actuators_[i]->get_force_B();

  // Transfer to Global Frame
  double x = cos(att_)*force.get_x() - sin(att_)*force.get_y();
  double y = sin(att_)*force.get_x() + cos(att_)*force.get_y();

  //std::cout << ' ' << x << ' ' << y << ' ' << att_ << ' ' << mass_ << std::endl;

  return Vec2(x, y);
}
Exemplo n.º 16
0
void ForcesWindow::RefreshWindow()
{
	HWND hWnd = getHandle();
	GuiMapPtr map = chkd.maps.curr;
	if ( map != nullptr )
	{
		for ( int force=0; force<4; force++ )
		{
			ChkdString forceName;
			bool allied = false, vision = false, random = false, av = false;
			map->getForceString(forceName, force);
			map->getForceInfo(force, allied, vision, random, av);

			SetWindowText(GetDlgItem(hWnd, EDIT_F1NAME+force), forceName.c_str());
			if ( allied ) SendMessage(GetDlgItem(hWnd, CHECK_F1ALLIED+force), BM_SETCHECK, BST_CHECKED  , 0);
			else		  SendMessage(GetDlgItem(hWnd, CHECK_F1ALLIED+force), BM_SETCHECK, BST_UNCHECKED, 0);
			if ( vision ) SendMessage(GetDlgItem(hWnd, CHECK_F1VISION+force), BM_SETCHECK, BST_CHECKED  , 0);
			else		  SendMessage(GetDlgItem(hWnd, CHECK_F1VISION+force), BM_SETCHECK, BST_UNCHECKED, 0);
			if ( random ) SendMessage(GetDlgItem(hWnd, CHECK_F1RANDOM+force), BM_SETCHECK, BST_CHECKED  , 0);
			else		  SendMessage(GetDlgItem(hWnd, CHECK_F1RANDOM+force), BM_SETCHECK, BST_UNCHECKED, 0);
			if ( av		) SendMessage(GetDlgItem(hWnd, CHECK_F1AV	 +force), BM_SETCHECK, BST_CHECKED  , 0);
			else		  SendMessage(GetDlgItem(hWnd, CHECK_F1AV	 +force), BM_SETCHECK, BST_UNCHECKED, 0);
		}

		for ( int i=0; i<4; i++ )
		{
			HWND hListBox = GetDlgItem(hWnd, LB_F1PLAYERS+i);
			if ( hListBox != NULL )
				while ( SendMessage(hListBox, LB_DELETESTRING, 0, 0) != LB_ERR );
		}

		for ( int player=0; player<8; player++ )
		{
			u8 force(0), color(0), race(0), displayOwner(map->getDisplayOwner(player));
			if ( map->getPlayerForce(player, force) )
			{
				map->getPlayerColor(player, color);
				map->getPlayerRace(player, race);
				std::stringstream ssplayer;
				ssplayer << "Player " << player+1 << " - " << playerColors.at(color) << " - "
						 << playerRaces.at(race) << " (" << playerOwners.at(displayOwner) << ")";
				HWND hListBox = GetDlgItem(hWnd, LB_F1PLAYERS+force);
				if ( hListBox != NULL )
					SendMessage(hListBox, LB_ADDSTRING, 0, (LPARAM)ssplayer.str().c_str());
			}
		}
	}
}
Exemplo n.º 17
0
Foam::dimensionedVector Foam::sixDOFqODE::A
(
    const dimensionedVector& xR,
    const dimensionedVector& uR,
    const HamiltonRodriguezRot& rotation
) const
{
    return
    (
       - (linSpringCoeffs_ & xR)    // spring
       - (linDampingCoeffs_ & uR)   // damping
       + force()
         // To absolute
       + (rotation.invR() & forceRelative())
    )/mass_;
}
/**
 * This is the method that does the heavy lifting in this class.
 * Called by step, it calculates what force the spring is experiencing,
 * and applies that force to the rigid bodies it connects to.
 */
void tgBulletCompressionSpring::calculateAndApplyForce(double dt)
{
    // Create variables to hold the results of these computations
    btVector3 force(0.0, 0.0, 0.0);
    // the following ONLY includes forces due to K, not due to damping.
    double magnitude = getSpringForce();

    // hold these variables so we don't have to call the accessor function twice.
    const double currLength = getCurrentSpringLength();
    const btVector3 unitVector = getAnchorDirectionUnitVector();

    // Calculate the damping force for this timestep.
    // Take an approximated derivative to estimate the velocity of the
    // tip of the compression spring:
    // TO-DO: MAKE THIS A BETTER APPROXIMATION TO THE DERIVATIVE!!
    const double changeInDeltaX = currLength - m_prevLength;
    m_velocity = changeInDeltaX / dt;

    // The damping force for this timestep
    // Like with spring force, a positive velocity should result in a
    // force acting against the movement of the tip of the spring.
    m_dampingForce = - getCoefD() * m_velocity;

    // Debugging
    #if (0)
      std::cout << "Length: " << getCurrentSpringLength() << " rl: " << getRestLength() <<std::endl;
      std::cout << "SpringForce: " << magnitude << " DampingForce: " << m_dampingForce <<std::endl;
    #endif

    // Add the damping force to the force from the spring.
    magnitude += m_dampingForce;
    
    // Project the force into the direction of the line between the two anchors
    force = unitVector * magnitude; 
    
    // Store the calculated length as the previous length
    m_prevLength = currLength;
    
    //Now Apply it to the connected two bodies
    btVector3 point1 = this->anchor1->getRelativePosition();
    this->anchor1->attachedBody->activate();
    this->anchor1->attachedBody->applyImpulse(force*dt,point1);
    
    btVector3 point2 = this->anchor2->getRelativePosition();
    this->anchor2->attachedBody->activate();
    this->anchor2->attachedBody->applyImpulse(-force*dt,point2);
}
Exemplo n.º 19
0
void dgCollisionInstance::CalculateBuoyancyAcceleration (const dgMatrix& matrix, const dgVector& origin, const dgVector& gravity, const dgVector& fluidPlane, dgFloat32 fluidDensity, dgFloat32 fluidViscosity, dgVector& unitForce, dgVector& unitTorque)
{
	dgMatrix globalMatrix (m_localMatrix * matrix);

	unitForce = dgVector (dgFloat32 (0.0f));
	unitTorque = dgVector (dgFloat32 (0.0f));
	dgVector volumeIntegral (m_childShape->CalculateVolumeIntegral (globalMatrix, fluidPlane, *this));
	if (volumeIntegral.m_w > dgFloat32 (0.0f)) {
		dgVector buoyanceCenter (volumeIntegral - origin);

		dgVector force (gravity.Scale (-fluidDensity * volumeIntegral.m_w));
		dgVector torque (buoyanceCenter.CrossProduct(force));

		unitForce += force;
		unitTorque += torque;
	}
}
Exemplo n.º 20
0
	DPoint SpringEmbedderGridVariant::ForceModelFRModAttr::computeDisplacement(int j, double boxLength) const
	{
		const double cEps = eps();

		const NodeInfo &vj = m_vInfo[j];
		int grid_x = vj.m_gridX;
		int grid_y = vj.m_gridY;

		// repulsive forces on node j: F_rep(d) = iel^3 / d
		DPoint force(0,0);
		for(int gi = -1; gi <= 1; gi++) {
			for(int gj = -1; gj <= 1; gj++) {
				for(int u : m_gridCell(grid_x+gi,grid_y+gj)) {

					if(u == j) continue;
					DPoint dist = vj.m_pos - m_vInfo[u].m_pos;
					double d = dist.norm();

					if(d < boxLength) {
						dist /= d * d + cEps;
						force += dist;
					}
				}
			}
		}

		force *= m_idealEdgeLength * m_idealEdgeLength * m_idealEdgeLength;
		DPoint disp(force);

		// attractive forces on j: F_attr(d) = -d^3 / iel
		force = DPoint(0,0);
		for(int l = vj.m_adjBegin; l != vj.m_adjStop; ++l) {
			int u = m_adjLists[l];

			DPoint dist = vj.m_pos - m_vInfo[u].m_pos;
			double d = dist.norm();

			dist *= d*d;
			force -= dist;
		}

		force /= m_idealEdgeLength;
		disp += force;

		return disp;
	}
Exemplo n.º 21
0
/******************************************************************************
 * Beräkna hjälplutningen k1 för samtliga objekt.
 * k1 är egentligen 2 hjälplutningar, för hastighets- och positions-beräkningen
 * Utnyttja Newton III för att undvika dubbla beräkningar
 *****************************************************************************/
void Cel_bodies::calculate_k1()
{
    Cel_bodies *current = this->next;
    
    vec3 F, zero;
    zero = vec3(0.0, 0.0, 0.0);
    while(current != NULL){
        F = force(current, 0.0, zero, zero);
        this->planet->kv1 = this->planet->kv1 + F/this->planet->mass;
        current->planet->kv1 = current->planet->kv1 - F/current->planet->mass; 


        current = current->next;
    }
    this->planet->kr1 = this->planet->velocity;

}
Exemplo n.º 22
0
Ref<Event> MouseEvent::cloneFor(HTMLIFrameElement* iframe) const
{
    ASSERT(iframe);
    Frame* frame = iframe->document().frame();
    FrameView* frameView = frame ? frame->view() : nullptr;
    Ref<MouseEvent> clonedMouseEvent = MouseEvent::create(type(), bubbles(), cancelable(),
        iframe->document().defaultView(),
        detail(), screenX(), screenY(),
        frameView ? adjustedClientX(clientX(), iframe, frameView) : 0,
        frameView ? adjustedClientY(clientY(), iframe, frameView) : 0,
        ctrlKey(), altKey(), shiftKey(), metaKey(),
        button(),
        // Nullifies relatedTarget.
        0);
    clonedMouseEvent->setForce(force());
    return WTFMove(clonedMouseEvent);
}
Exemplo n.º 23
0
QValidator::State CQValidatorBound::validate(QString & input, int & pos) const
{
  QString Input;

  if (input == mValidBound ||
      mpDoubleValidator->validate(input, pos) == Acceptable ||
      (input.startsWith(mSign) &&
       input.endsWith("%") &&
       mpDoubleValidator->validate(Input = input.mid(1, input.length() - 2), pos)))
    {
      force(input);
      return Acceptable;
    }

  setColor(Invalid);
  return Intermediate;
}
Exemplo n.º 24
0
/*!
 * Return force tensor to be applied to the attached object.
 */
vpColVector vpVirtuose::getForce() const
{
  if (!m_is_init) {
    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
  }

  vpColVector force(6,0);
  float force_[6];
  if (virtGetForce(m_virtContext, force_)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtGetForce: error code %d", err));
  }

  for(unsigned int i=0; i<6; i++)
    force[i] = force_[i];
  return force;
}
void ForceRendererTest::arbitrary3D() {
    Vector3 force(3.7f, -5.7f, -11.5f);
    Matrix4 m = Implementation::forceRendererTransformation<3>({0.5f, -3.0f, 1.0f}, force);

    /* Translation, right-pointing base vector is the same as force */
    CORRADE_COMPARE(m.translation(), Vector3(0.5f, -3.0f, 1.0f));
    CORRADE_COMPARE(m.right(), force);

    /* All vectors have the same length */
    CORRADE_COMPARE(m.up().length(), force.length());
    CORRADE_COMPARE(m.backward().length(), force.length());

    /* All vectors are orthogonal */
    CORRADE_COMPARE(Math::dot(m.right(), m.up()),       0.0f);
    CORRADE_COMPARE(Math::dot(m.right(), m.backward()), 0.0f);
    /** @todo This shouldn't be too different */
    CORRADE_VERIFY(Math::abs(Math::dot(m.up(), m.backward())) < Math::TypeTraits<Float>::epsilon());
}
Exemplo n.º 26
0
extern "C" sfcgal_geometry_t* sfcgal_geometry_force_rhr( const sfcgal_geometry_t* ga ) 
{
	const SFCGAL::Geometry* g = reinterpret_cast<const SFCGAL::Geometry*>(ga);
	SFCGAL::Geometry* gb = g->clone();
	SFCGAL::transform::ForceOrderPoints force( /* ccw */ false );
	try
	{
		gb->accept( force );
	}
	catch ( std::exception& e )
	{
		SFCGAL_WARNING( "During force_rhr(A) :" );
		SFCGAL_WARNING( "  with A: %s", ((const SFCGAL::Geometry*)(ga))->asText().c_str() );
		SFCGAL_ERROR( "%s", e.what() );
		return 0;
	}
	return gb;
}
Exemplo n.º 27
0
void NonlinearForceDensityNoDeriv::force_value(
                                const FEMesh *mesh, const Element *element,
				const Equation *eqn, const MasterPosition &point,
				double time, SmallSystem *eqndata) const
{
  DoubleVec fieldVal(3), force(3);
  Coord coord;

  // first compute the current value of the displacement field at the gauss point

  fieldVal[0] = fieldVal[1] = 0.0;
#if DIM==3
  fieldVal[2] = 0.0;
#endif
  for(CleverPtr<ElementFuncNodeIterator> node(element->funcnode_iterator());
      !node->end(); ++*node) {
    double shapeFuncVal = node->shapefunction( point );
    fieldVal[0] += shapeFuncVal * (*displacement)( *node, 0 )->value( mesh );
    fieldVal[1] += shapeFuncVal * (*displacement)( *node, 1 )->value( mesh );
#if DIM==3
    fieldVal[2] += shapeFuncVal * (*displacement)( *node, 2 )->value( mesh );
#endif
  }

  // now compute the force density value for the current coordinate x,y,z,
  // time and displacement,
  // the nonlinear force density function returns the corresponding
  // force value in the array 'force',

  coord = element->from_master( point );

#if DIM==2
  nonlin_force_density( coord.x, coord.y, 0.0, time, fieldVal, force );
#elif DIM==3
  nonlin_force_density( coord.x, coord.y, coord.z, time, fieldVal, force );
#endif

  eqndata->force_vector_element(0) -= force[0];
  eqndata->force_vector_element(1) -= force[1];
#if DIM==3
  eqndata->force_vector_element(2) -= force[2];
#endif

} // end of 'NonlinearForceDensityNoDeriv::force_value'
Exemplo n.º 28
0
	TEST_FIXTURE(RigidBodyFixture, IntegrationAccuracyTest)
	{
//		DN_NOT_IMPLEMENTED;

		float h = 0.01;
		dynamx::Point3 pos(1.0f, 5.0f, -3.0f);
		dynamx::Vector3 force(0.0f, -10.0f, 0.4f);
/*
		rigidbody.SetPos(pos);
		rigidbody.ClearForces();
		rigidbody.AddForce(force);
*/
		rigidbody.Integrate(h);
		/*
		 * TODO : Runge kutta 4th order.
		//RigidBody should have travelled
		//Using 4th Order Runge Kutta method:
		//x(t0 + h) = x0 + 1/6(k1) + 1/3(k2) + 1/3(k3) + 1/6(k4)
		// where:
		// 	h is stepsize
		// 	k1 = h * f (x0, t0)
		// 	k2 = h * f (x0 + (k1/2), t0 + (h/2))
		// 	k3 = h * f (x0 + (k2/2), t0 + (h/2))
		// 	k4 = h * f (x0 + k3, t0 + h)
		//Error is :
		//	O(h^3)
		*/

/*
		//Euler integration.		
		CHECK_CLOSE(pos.GetX() + (h * force.GetX()), 
				rigidbody.GetPos().GetX(), 
				DN_CLOSE_FLOAT);

		CHECK_CLOSE(pos.GetY() + (h * force.GetY()), 
				rigidbody.GetPos().GetY(), 
				DN_CLOSE_FLOAT);

		CHECK_CLOSE(pos.GetZ() + (h * force.GetZ()), 
				rigidbody.GetPos().GetZ(), 
				DN_CLOSE_FLOAT);
*/

	}
Exemplo n.º 29
0
	void forceInit()
	{
		GotoXY(1, 19);
		printf("                                  ");
		GotoXY(1, 19);
		printf("正在进行第二步处理...");
		int x = 0;
		isok = false;
		for (int i = 1; i < 10; i++)
		{
			for (int j = 1; j < 10; j++)
			{
				sz[x++] = SUDOK[i][j].result;
			}
		}
		SetColor(6);
		force(0);
		SetColor(10);
	}
Exemplo n.º 30
0
void HuntCrossleyForce::extendAddToSystem(SimTK::MultibodySystem& system) const
{
    Super::extendAddToSystem(system);

    const ContactParametersSet& contactParametersSet = 
        get_contact_parameters();
    const double& transitionVelocity = get_transition_velocity();

    SimTK::GeneralContactSubsystem& contacts = system.updContactSubsystem();
    SimTK::ContactSetIndex set = contacts.createContactSet();
    SimTK::HuntCrossleyForce force(_model->updForceSubsystem(), contacts, set);
    force.setTransitionVelocity(transitionVelocity);
    for (int i = 0; i < contactParametersSet.getSize(); ++i)
    {
        ContactParameters& params = contactParametersSet.get(i);
        for (int j = 0; j < params.getGeometry().size(); ++j) {
            // get the ContactGeometry from the Model
            const ContactGeometry& geom =
                getModel().getComponent<ContactGeometry>(params.getGeometry()[j]);

            // B: base Frame (Body or Ground)
            // F: PhysicalFrame that this ContactGeometry is connected to
            // P: the frame defined (relative to F) by the location and
            //    orientation properties.
            const auto& X_BF = geom.getFrame().findTransformInBaseFrame();
            const auto& X_FP = geom.getTransform();
            const auto X_BP = X_BF * X_FP;
            contacts.addBody(set, geom.getFrame().getMobilizedBody(),
                    geom.createSimTKContactGeometry(), X_BP);
            force.setBodyParameters(
                    SimTK::ContactSurfaceIndex(contacts.getNumBodies(set)-1),
                    params.getStiffness(), params.getDissipation(),
                    params.getStaticFriction(), params.getDynamicFriction(),
                    params.getViscousFriction());
        }
    }

    // Beyond the const Component get the index so we can access the
    // SimTK::Force later.
    HuntCrossleyForce* mutableThis = const_cast<HuntCrossleyForce *>(this);
    mutableThis->_index = force.getForceIndex();
}