static int initint (TrapContext *ctx)
{
	uae_u32 tmp1;
	uaecptr p;

	if (irq_init)
		return 1;
	m68k_dreg (regs, 0) = 26;
	m68k_dreg (regs, 1) = 65536 + 1;
	p = CallLib (ctx, get_long (4), -0xC6); /* AllocMem */
	if (!p)
		return 0;
	tmp1 = here ();
	calltrap (deftrap2 (uaenet_int_handler, TRAPFLAG_EXTRA_STACK, _T("uaenet_int_handler")));
	put_word (p + 8, 0x020a);
	put_long (p + 10, ROM_netdev_resid);
	put_long (p + 18, tmp1);
	m68k_areg (regs, 1) = p;
	m68k_dreg (regs, 0) = 3; /* PORTS */
	dw (0x4a80); /* TST.L D0 */
	dw (0x4e75); /* RTS */
	CallLib (ctx, get_long (4), -168); /* AddIntServer */
	irq_init = 1;
	return 1;
}

void InsetMathBinom::draw(PainterInfo & pi, int x, int y) const
{
	Dimension const dim = dimension(*pi.base.bv);
	Dimension const & dim0 = cell(0).dimension(*pi.base.bv);
	Dimension const & dim1 = cell(1).dimension(*pi.base.bv);
	// define the binom brackets
	docstring const bra = kind_ == BRACE ? from_ascii("{") :
		kind_ == BRACK ? from_ascii("[") : from_ascii("(");
	docstring const ket = kind_ == BRACE ? from_ascii("}") :
		kind_ == BRACK ? from_ascii("]") : from_ascii(")");

	int m = x + dim.width() / 2;
	// FIXME: for an unknown reason the cells must be drawn directly
	// after the StyleChanger and cannot be drawn after the if case
	if (kind_ == DBINOM) {
		StyleChanger dummy(pi.base, LM_ST_DISPLAY);
		cell(0).draw(pi, m - dim0.wid / 2, y - dim0.des - 3 - 5);
		cell(1).draw(pi, m - dim1.wid / 2, y + dim1.asc + 3 - 5);
	} else if (kind_ == TBINOM) {
		StyleChanger dummy(pi.base, LM_ST_SCRIPT);
		cell(0).draw(pi, m - dim0.wid / 2, y - dim0.des - 3 - 5);
		cell(1).draw(pi, m - dim1.wid / 2, y + dim1.asc + 3 - 5);
	} else {
		FracChanger dummy2(pi.base);
		cell(0).draw(pi, m - dim0.wid / 2, y - dim0.des - 3 - 5);
		cell(1).draw(pi, m - dim1.wid / 2, y + dim1.asc + 3 - 5);
	}
	// draw the brackets and the marker
	mathed_draw_deco(pi, x, y - dim.ascent(), dw(dim.height()),
		dim.height(), bra);
	mathed_draw_deco(pi, x + dim.width() - dw(dim.height()),
		y - dim.ascent(), dw(dim.height()), dim.height(), ket);
	drawMarkers2(pi, x, y);
}

void wavhdr(void*m,UL hz,UL dlen){
 WAVHDR*p=m;
 p->riff=dw("RIFF");
 p->len=dlen+44;
 p->wave=dw("WAVE");
 p->fmt=dw("fmt ");
 p->flen=0x10;
 p->one=1;
 p->chan=1;
 p->hz=hz;
 p->bpsec=hz;
 p->bpsmp=1;
 p->bitpsmp=8;
 p->dat=dw("data");
 p->dlen=dlen;
}

TEST( Types, basic_dword_put ) {
	util::Buffer dw(1);
	dw.resize(4);
	PUT_DWORD(0x01020304L, dw.buffer());
	ASSERT_EQ( "04:03:02:01", dw.asHexa() );

}

STKUNIT_UNIT_TEST(geom, volume)
{
  dw().m(LOG_GEOMETRY_VERIFIER) << "TEST::geom::volume " << stk_classic::diag::dendl;

  const size_t num_x = 3;
  const size_t num_y = 3;
  const size_t num_z = 3;
  std::string config_mesh =
    Ioss::Utils::to_string(num_x) + "x" +
    Ioss::Utils::to_string(num_y) + "x" +
    Ioss::Utils::to_string(num_z) + "|bbox:0,0,0,1,1,1";
	
  PerceptMesh eMesh(3u);
  eMesh.new_mesh(GMeshSpec(config_mesh));
  eMesh.commit();
  // no need for this in create mode: eMesh.readBulkData();

  //FEMMetaData& metaData = *eMesh.get_fem_meta_data();
  mesh::BulkData& bulkData = *eMesh.get_bulk_data();

  eMesh.dump();
  GeometryVerifier geomVerifier(false);
  geomVerifier.isGeometryBad(bulkData, true);
  //setDoPause(true);
  //pause();
}

uaecptr libemu_InstallFunctionFlags (TrapFunction f, uaecptr libbase, int offset,
				     int flags, const char *tracename)
{
    int i;
    uaecptr retval;
    uaecptr execbase = get_long (four);
    int trnum;
    uaecptr addr = here();
    calltrap (trnum = deftrap2 (f, flags, tracename));
    dw (RTS);

    _68k_areg(1) = libbase;
    _68k_areg(0) = offset;
    _68k_dreg(0) = addr;
    retval = CallLib (execbase, -420);

    trapoldfunc[trnum] = retval;
#if 0
    for (i = 0; i < n_libpatches; i++) {
	if (libpatches[i].libbase == libbase)
	    break;
    }
    if (i == n_libpatches) {
	int j;
	libpatches[i].libbase = libbase;
	for (j = 0; j < 300; j++)
	    libpatches[i].functions[j] = NULL;
	n_libpatches++;
    }
    libpatches[i].functions[-offset/6] = f;
#endif
    return retval;
}

void
serial_write_block(unsigned char *ch, int len)
{
#if 1
  while (len--)
    serial_write(*ch++);
  serial_sync ();
#else
  DWORD num;
  if (verbose > 1)
    {
      int i;
      printf("\033[36m[%d]\033[0m", len);
      for (i=0; i<len; i++)
	dw (ch[i]);
    }
  while (len > 32)
    {
      FT_Write (handle, ch, 32, &num);
      ch += 32;
      len -= 32;
      serial_sync ();
    }
  if (len)
    FT_Write (handle, ch, len, &num);
  serial_sync ();
#endif
}

void TestDiagram::slotTestDeleteItemById()
{

    Ide ide;

    QDomDocument doc;

    DiagramWindow dw(&doc,&ide);

    // test data element
    int id = 1;
    int type = 1;

    dw.addData("newData",id);

    dw.deleteItemById(id,type);

    QVERIFY(dw.data.isEmpty());

    // test task element
    id = 1;
    type = 0;

    dw.addTask("newTask",1,1,id);
    dw.deleteItemById(id,type);

    QVERIFY(dw.tasks.isEmpty());
}

//
// IActiveScript
//
HRESULT STDMETHODCALLTYPE CRScriptCore::SetScriptSite( 
            /* [in] */ IActiveScriptSite __RPC_FAR *pass)
{
	if (!pass) return E_POINTER;
	if (m_pSite) return E_UNEXPECTED;

	MakeScope();

	pass->AddRef();
	m_pSite = pass;

#ifdef __IRubyWrapper_INTERFACE_DEFINED__
	if (m_dwSafety & INTERFACE_USES_SECURITY_MANAGER)
	{
		IServiceProvider* pProv = NULL;
		HRESULT hr = m_pSite->QueryInterface(IID_IServiceProvider, (void**)&pProv);
		if (hr == S_OK)
		{
			IInternetHostSecurityManager* pScm = NULL;
			hr = pProv->QueryService(SID_SInternetHostSecurityManager, IID_IInternetHostSecurityManager, (void**)&pScm);
			if (hr == S_OK)
			{
				BYTE b[_MAX_PATH];
				DWORD dw(sizeof(b));
				hr = pScm->GetSecurityId(b, &dw, 0);
				m_pScM = pScm;
			}
			m_pProv = pProv;
		}
	}
#endif
	return S_OK;
}

Vector ADFun<Base>::RevOne(const Vector  &x, size_t i)
{	size_t i1;

	size_t n = Domain();
	size_t m = Range();

	// check Vector is Simple Vector class with Base type elements
	CheckSimpleVector<Base, Vector>();

	CppADUsageError(
		x.size() == n,
		"RevOne: Length of x not equal domain dimension for f"
	); 
	CppADUsageError(
		i < m,
		"RevOne: the index i is not less than range dimension for f"
	);

	// point at which we are evaluating the derivative
	Forward(0, x);

	// component which are are taking the derivative of
	Vector w(m);
	for(i1 = 0; i1 < m; i1++)
		w[i1] = 0.;
	w[i] = Base(1);

	// dimension the return value
	Vector dw(n);

	// compute the return value
	dw = Reverse(1, w);

	return dw;
}

int
main(int argc, char* argv[])
{
  tspub_options opt;
  try {
    // Parse the command line args
    opt = parse_tspub_args(argc, argv);
  }
  catch (...) { }
    // Initialize random number generation with a seed
    srandom(clock());
    // Create a SimD runtime
    dds::Runtime runtime("");
    // Create the "TempSensor" Topic
    dds::Topic<TempSensorType> tsTopic("TempSensor");
    // Create a DataWriter
    dds::DataWriter<TempSensorType> dw(tsTopic);
    
    // Write some temperature randomly changing around a set point
    float temp = opt.t0 + ((random()*opt.dt)/RAND_MAX);
    float hum  = opt.h0 + ((random()*opt.dh)/RAND_MAX);
    TempSensorType sensor = { opt.id, temp, hum, opt.scale };
    for (unsigned int i = 0; i < opt.samples; ++i) {
      dw.write(sensor);
      std::cout << "." << std::flush;
      nanosleep(&opt.period, 0);
      sensor.temp = opt.t0 + ((random()*opt.dt)/RAND_MAX);
      sensor.hum =  opt.h0 + ((random()*opt.dh)/RAND_MAX);
    }
    std::cout << std::endl;
  return 0;
}

/**
 * This function performs the publisher role in this example.
 * @return 0 if a sample is successfully written, 1 otherwise.
 */
int publisher(int argc, char *argv[])
{
    int result = 0;
    try
    {
        /** A dds::domain::DomainParticipant is created for the default domain. */
        dds::domain::DomainParticipant dp(org::opensplice::domain::default_id());

        /** The Durability::Transient policy is specified as a dds::topic::qos::TopicQos
         * so that even if the subscriber does not join until after the sample is written
         * then the DDS will still retain the sample for it. The Reliability::Reliable
         * policy is also specified to guarantee delivery. */
        dds::topic::qos::TopicQos topicQos
             = dp.default_topic_qos()
                << dds::core::policy::Durability::Transient()
                << dds::core::policy::Reliability::Reliable();

        /** A dds::topic::Topic is created for our sample type on the domain participant. */
        dds::topic::Topic<HelloWorldData::Msg> topic(dp, "HelloWorldData_Msg", topicQos);

        /** A dds::pub::Publisher is created on the domain participant. */
        std::string name = "HelloWorld example";
        dds::pub::qos::PublisherQos pubQos
            = dp.default_publisher_qos()
                << dds::core::policy::Partition(name);
        dds::pub::Publisher pub(dp, pubQos);

        /** The dds::pub::qos::DataWriterQos is derived from the topic qos and the
         * WriterDataLifecycle::ManuallyDisposeUnregisteredInstances policy is
         * specified as an addition. This is so the publisher can optionally be run (and
         * exit) before the subscriber. It prevents the middleware default 'clean up' of
         * the topic instance after the writer deletion, this deletion implicitly performs
         * DataWriter::unregister_instance */
        dds::pub::qos::DataWriterQos dwqos = topic.qos();
        dwqos << dds::core::policy::WriterDataLifecycle::ManuallyDisposeUnregisteredInstances();

        /** A dds::pub::DataWriter is created on the Publisher & Topic with the modififed Qos. */
        dds::pub::DataWriter<HelloWorldData::Msg> dw(pub, topic, dwqos);

        /** A sample is created and then written. */
        HelloWorldData::Msg msgInstance(1, "Hello World");
        dw << msgInstance;

        std::cout << "=== [Publisher] written a message containing :" << std::endl;
        std::cout << "    userID  : " << msgInstance.userID() << std::endl;
        std::cout << "    Message : \"" << msgInstance.message() << "\"" << std::endl;

        /* A short sleep ensures time is allowed for the sample to be written to the network.
        If the example is running in *Single Process Mode* exiting immediately might
        otherwise shutdown the domain services before this could occur */
        exampleSleepMilliseconds(1000);
    }
    catch (const dds::core::Exception& e)
    {
        std::cerr << "ERROR: Exception: " << e.what() << std::endl;
        result = 1;
    }
    return result;
}

double
QualPCR::evaluate()
{
  Matrix dw(&mWrench[0], 6, 1, true);
  double wrenchMultiplier = mWrenchMultiplier;
  if (mWrenchMultiplier) dw.multiply(wrenchMultiplier);

  return evaluatePCR(grasp, dw, mMaxForce);
}

void tet_hp::element_jacobian(int tind, Array<FLT,2> &K) {
	Array<TinyVector<FLT,MXTM>,1> R(NV),Rbar(NV),lf_re(NV),lf_im(NV);
	Array<FLT,1> dw(NV);
#ifdef BZ_DEBUG
	const FLT eps_r = 0.0e-6, eps_a = 1.0e-6;  /*<< constants for debugging jacobians */
#else
	const FLT eps_r = 1.0e-6, eps_a = 1.0e-10;  /*<< constants for accurate numerical determination of jacobians */
#endif
	
	ugtouht(tind);
	
	dw = 0.0;
	for(int i=0;i<4;++i)
		for(int n=0;n<NV;++n)
			dw = dw + fabs(uht(n)(i));
	
	dw = dw*eps_r;
	dw = dw +eps_a;
	
	element_rsdl(tind,0,uht,lf_re,lf_im);
	for(int i=0;i<basis::tet(log2p).tm;++i) 
		for(int n=0;n<NV;++n) 
			Rbar(n)(i)=lf_re(n)(i)+lf_im(n)(i);
	
	
	int kcol = 0;
	for(int mode = 0; mode < basis::tet(log2p).tm; ++mode){
		for(int var = 0; var < NV; ++var){
			uht(var)(mode) += dw(var);
			
			element_rsdl(tind,0,uht,lf_re,lf_im);
			
			int krow = 0;
			for(int i=0;i<basis::tet(log2p).tm;++i)
				for(int n=0;n<NV;++n)
					K(krow++,kcol) = (lf_re(n)(i) +lf_im(n)(i) -Rbar(n)(i))/dw(var);
			
			++kcol;
			uht(var)(mode) -= dw(var);
		}
	}
	
	return;
}

int Registry(int isitget, const char *keyName, char *ItemName, char *buffer, char *errstr)
{
	bool done(false);
    char achClass[MAX_PATH], achValue[MAX_VALUE_NAME];
    DWORD  cchClassName(MAX_PATH), cSubKeys(0), cbMaxSubKey, cchMaxClass, cValues, cchMaxValue, cbMaxValueData, cbSecurityDescriptor, cchValue(MAX_VALUE_NAME);
	DWORD  retCode, i;
    FILETIME ftLastWriteTime;      // last write time 

	HKEY key;
	DWORD dw(0), dwres, dwType, cbData(MAX_PATH);
	BYTE btdata[MAX_PATH];
	achClass[0]='\0'; 

	LONG ln = RegCreateKeyEx( HKEY_CURRENT_USER, keyName, 0, NULL, 0, KEY_ALL_ACCESS, NULL, &key, &dwres);
	if (!isitget) // Set values in the registry 
	{
		if (RegSetValueEx(key, ItemName, 0, REG_SZ, (const BYTE*)&buffer[0], (DWORD)strlen(buffer))!=ERROR_SUCCESS)
		{	strcpy(errstr,"error on RegSetValueEx()"); return 0;  }
	}
	else // Get values from the registry 
	{
		if (dwres==REG_CREATED_NEW_KEY)
		{	strcpy(errstr,"The registry key does not exist (just created now)"); return 0; }
		else if (dwres==REG_OPENED_EXISTING_KEY)
		{
			retCode = RegQueryInfoKey(key, achClass, &cchClassName, NULL,
				&cSubKeys, &cbMaxSubKey, &cchMaxClass, &cValues, &cchMaxValue, 
				&cbMaxValueData, &cbSecurityDescriptor, &ftLastWriteTime);
			if (cValues) 
			{
				for (i=0, retCode=ERROR_SUCCESS; i<cValues; i++) 
				{ 
					achValue[0] = '\0'; 
					cchValue = cbData = MAX_PATH;
					retCode = RegEnumValue(key, i, achValue, &cchValue, NULL, &dwType, btdata, &cbData);
					if (retCode == ERROR_SUCCESS) 
					{ 
						if (!strcmp(achValue,ItemName))
						{
							done = true;
							strcpy(buffer, (LPCSTR)btdata);
						}
					}
					else
					{   strcpy(errstr,"error on RegEnumValue()"); return 0;    }
				}
				if (!done)  { sprintf(errstr,"The registry key item %s does not exist.",ItemName ); return 0; }
			}
			else
			{	sprintf(errstr,"No key associated with %s found.", keyName); return 0; }
		}
	}
	RegCloseKey(key);
	return 1;
}

void dgBody::AddImpulse (const dgVector& pointDeltaVeloc, const dgVector& pointPosit)
{
	dgMatrix invInertia (CalculateInvInertiaMatrix());

	// get contact matrix
	dgMatrix tmp;
	dgVector globalContact (pointPosit - m_globalCentreOfMass);

	tmp[0][0] = dgFloat32 (0.0f);
	tmp[0][1] = + globalContact[2];
	tmp[0][2] = - globalContact[1];
	tmp[0][3] = dgFloat32 (0.0f);

	tmp[1][0] = -globalContact[2];
	tmp[1][1] = dgFloat32 (0.0f);
	tmp[1][2] = +globalContact[0];
	tmp[1][3] = dgFloat32 (0.0f);

	tmp[2][0] = +globalContact[1];
	tmp[2][1] = -globalContact[0];
	tmp[2][2] = dgFloat32 (0.0f);
	tmp[2][3] = dgFloat32 (0.0f);

	tmp[3][0] = dgFloat32 (0.0f);
	tmp[3][1] = dgFloat32 (0.0f);
	tmp[3][2] = dgFloat32 (0.0f);
	tmp[3][3] = dgFloat32 (1.0f);

	dgMatrix contactMatrix (tmp * invInertia * tmp);
	for (dgInt32 i = 0; i < 3; i ++) {
		for (dgInt32 j = 0; j < 3; j ++) {
			contactMatrix[i][j] *= -dgFloat32 (1.0f);
		}
	}
	contactMatrix[0][0] += m_invMass.m_w;	
	contactMatrix[1][1] += m_invMass.m_w;	
	contactMatrix[2][2] += m_invMass.m_w;	

	contactMatrix = contactMatrix.Symetric3by3Inverse ();

	// change of momentum
	dgVector changeOfMomentum (contactMatrix.RotateVector (pointDeltaVeloc));


	dgVector dv (changeOfMomentum.Scale3 (m_invMass.m_w));
	dgVector dw (invInertia.RotateVector (globalContact * changeOfMomentum));

	m_veloc += dv;
	m_omega += dw;

	m_sleeping	= false;
	m_equilibrium = false;
	Unfreeze ();
}

int
main(int argc, char* argv[]) 
{
    if (argc < 2) {
        std::cout << "USAGE:\n\t qppub <sensor-id>" << std::endl;
        return -1;
    }

    int sid = atoi(argv[1]);
    const int N = 100;

   /*segment1-start*/
   dds::core::QosProvider qp("file://defaults.xml", "DDS DefaultQosProfile");

  // create a Domain Participant, -1 defaults to value defined in configuration file
   dds::domain::DomainParticipant dp(-1);

   dds::topic::qos::TopicQos topicQos = qp.topic_qos();

   dds::topic::Topic<tutorial::TempSensorType> topic(dp, "TempSensor", topicQos);

   dds::pub::qos::PublisherQos pubQos = qp.publisher_qos();
   dds::pub::Publisher pub(dp, pubQos);

   dds::pub::qos::DataWriterQos dwqos = qp.datawriter_qos();
   dds::pub::DataWriter<tutorial::TempSensorType> dw(pub, topic, dwqos);  
   /*segment1-end*/
 
    const float avgT = 25;
    const float avgH = 0.6;
    const float deltaT = 5;
    const float deltaH = 0.15;
    // Initialize random number generation with a seed
    srandom(clock());
    
    // Write some temperature randomly changing around a set point
    float temp = avgT + ((random()*deltaT)/RAND_MAX);
    float hum  = avgH + ((random()*deltaH)/RAND_MAX);

    tutorial::TempSensorType sensor( sid, temp, hum, tutorial::CELSIUS );

    // Write the data
    for (unsigned int i = 0; i < N; ++i) {
        dw.write(sensor);
        std::cout << "DW << " << sensor << std::endl;
        std::this_thread::sleep_for(std::chrono::seconds(1));
        temp = avgT + ((random()*deltaT)/RAND_MAX);
        sensor.temp(temp); 
        hum = avgH + ((random()*deltaH)/RAND_MAX);
        sensor.hum(hum);
    }
  
   return 0;
}

/*
 * Installs the UAE LIBRARY
 */
void emulib_install (void)
{
    uaecptr a = here ();
    org (RTAREA_BASE + 0xFF60);
//    dw (0x4eb9);
//    dw ((RTAREA_BASE >> 16) | get_word (RTAREA_BASE + 36));
//    dw (get_word (RTAREA_BASE + 38) + 12);
    calltrap (define_trap (uaelib_demux, 0, ""));
    dw (RTS);
    org (a);
}

void
serial_write (unsigned char ch)
{
  DWORD num;

  if (verbose > 1)
    dw (ch);
  FT_Write (handle, &ch, 1, &num);
  serial_sync ();
  usleep(100);
}

dgJacobian dgDynamicBody::IntegrateForceAndToque(const dgVector& force, const dgVector& torque, const dgVector& timestep)
{
	dgJacobian velocStep;
	if (m_gyroTorqueOn) {
		dgVector dtHalf(timestep * dgVector::m_half);
		dgMatrix matrix(m_gyroRotation, dgVector::m_wOne);

		dgVector localOmega(matrix.UnrotateVector(m_omega));
		dgVector localTorque(matrix.UnrotateVector(torque - m_gyroTorque));

		// derivative at half time step. (similar to midpoint Euler so that it does not loses too much energy)
		dgVector dw(localOmega * dtHalf);
		dgMatrix jacobianMatrix(
			dgVector(m_mass[0], (m_mass[2] - m_mass[1]) * dw[2], (m_mass[2] - m_mass[1]) * dw[1], dgFloat32(0.0f)),
			dgVector((m_mass[0] - m_mass[2]) * dw[2], m_mass[1], (m_mass[0] - m_mass[2]) * dw[0], dgFloat32(1.0f)),
			dgVector((m_mass[1] - m_mass[0]) * dw[1], (m_mass[1] - m_mass[0]) * dw[0], m_mass[2], dgFloat32(1.0f)),
			dgVector::m_wOne);

		// and solving for alpha we get the angular acceleration at t + dt
		// calculate gradient at a full time step
		//dgVector gradientStep(localTorque * timestep);
		dgVector gradientStep(jacobianMatrix.SolveByGaussianElimination(localTorque * timestep));

		dgVector omega(matrix.RotateVector(localOmega + gradientStep));
		dgAssert(omega.m_w == dgFloat32(0.0f));

		// integrate rotation here
		dgFloat32 omegaMag2 = omega.DotProduct(omega).GetScalar() + dgFloat32(1.0e-12f);
		dgFloat32 invOmegaMag = dgRsqrt(omegaMag2);
		dgVector omegaAxis(omega.Scale(invOmegaMag));
		dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep.GetScalar();
		dgQuaternion deltaRotation(omegaAxis, omegaAngle);
		m_gyroRotation = m_gyroRotation * deltaRotation;
		dgAssert((m_gyroRotation.DotProduct(m_gyroRotation) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));

		matrix = dgMatrix(m_gyroRotation, dgVector::m_wOne);
		localOmega = matrix.UnrotateVector(omega);
		//dgVector angularMomentum(inertia * localOmega);
		//body->m_gyroTorque = matrix.RotateVector(localOmega.CrossProduct(angularMomentum));
		//body->m_gyroAlpha = body->m_invWorldInertiaMatrix.RotateVector(body->m_gyroTorque);
		dgVector localGyroTorque(localOmega.CrossProduct(m_mass * localOmega));
		m_gyroTorque = matrix.RotateVector(localGyroTorque);
		m_gyroAlpha = matrix.RotateVector(localGyroTorque * m_invMass);

		velocStep.m_angular = matrix.RotateVector(gradientStep);
	} else {
		velocStep.m_angular = m_invWorldInertiaMatrix.RotateVector(torque) * timestep;
		//velocStep.m_angular = velocStep.m_angular * dgVector::m_half;
	}

	velocStep.m_linear = force.Scale(m_invMass.m_w) * timestep;
	return velocStep;
}