Exemplo n.º 1
1
/*
 * byte_bit_run_length_N[B], for 0 <= N <= 7, gives the length of the
 * run of 1-bits starting at bit N in a byte with value B,
 * numbering the bits in the byte as 01234567.  If the run includes
 * the low-order bit (i.e., might be continued into a following byte),
 * the run length is increased by 8.
 */

#define t8(n) n,n,n,n,n+1,n+1,n+2,n+11
#define r8(n) n,n,n,n,n,n,n,n
#define r16(n) r8(n),r8(n)
#define r32(n) r16(n),r16(n)
#define r64(n) r32(n),r32(n)
#define r128(n) r64(n),r64(n)
const byte byte_bit_run_length_0[256] = {
    r128(0), r64(1), r32(2), r16(3), r8(4), t8(5)
};
const byte byte_bit_run_length_1[256] = {
    r64(0), r32(1), r16(2), r8(3), t8(4),
    r64(0), r32(1), r16(2), r8(3), t8(4)
};
const byte byte_bit_run_length_2[256] = {
    r32(0), r16(1), r8(2), t8(3),
    r32(0), r16(1), r8(2), t8(3),
    r32(0), r16(1), r8(2), t8(3),
    r32(0), r16(1), r8(2), t8(3)
};
const byte byte_bit_run_length_3[256] = {
    r16(0), r8(1), t8(2), r16(0), r8(1), t8(2),
    r16(0), r8(1), t8(2), r16(0), r8(1), t8(2),
    r16(0), r8(1), t8(2), r16(0), r8(1), t8(2),
Exemplo n.º 2
0
void enable_port(void)
{
 unsigned long dat32;

 w32(HcRhP1,0x00000102);
 w32(HcRhP2,0x00000102);
 w32(HcRhStatus,0x00010000);	//set Global Power

 w32(HcRhA,0x20000102);
 w32(HcRhB,0x00000000);

 dat32=r32(HcRhP2);
 if((dat32&0x00000001)==1)
 {
  set_port_speed(2,0);
  if(((dat32)&(0x00000200))!=0)
  {
   set_port_speed(2,1);
  }
 }

 dat32=r32(HcRhP1);
 if((dat32&0x00000001)==1)
 {
  set_port_speed(1,0);
  if(((dat32)&(0x00000200))!=0)
  {
   set_port_speed(1,1);
  }
 }
}
Exemplo n.º 3
0
//--------------------------------------------------------------------------------------------------
bool q3Box::Raycast( const q3Transform& tx, q3RaycastData* raycast ) const
{
	q3Transform world = q3Mul( tx, local );
	q3Vec3 d = q3MulT( world.rotation, raycast->dir );
	q3Vec3 p = q3MulT( world, raycast->start );
	const r32 epsilon = r32( 1.0e-8 );
	r32 tmin = 0;
	r32 tmax = raycast->t;

	// t = (e[ i ] - p.[ i ]) / d[ i ]
	r32 t0;
	r32 t1;
	q3Vec3 n0;

	for ( int i = 0; i < 3; ++i )
	{
		// Check for ray parallel to and outside of AABB
		if ( q3Abs( d[ i ] ) < epsilon )
		{
			// Detect separating axes
			if ( p[ i ] < -e[ i ] || p[ i ] > e[ i ] )
			{
				return false;
			}
		}

		else
		{
			r32 d0 = r32( 1.0 ) / d[ i ];
			r32 s = q3Sign( d[ i ] );
			r32 ei = e[ i ] * s;
			q3Vec3 n( 0, 0, 0 );
			n[ i ] = -s;

			t0 = -(ei + p[ i ]) * d0;
			t1 = (ei - p[ i ]) * d0;

			if ( t0 > tmin )
			{
				n0 = n;
				tmin = t0;
			}

			tmax = q3Min( tmax, t1 );

			if ( tmin > tmax )
			{
				return false;
			}
		}
	}

	raycast->normal = q3Mul( world.rotation, n0 ); 
	raycast->toi = tmin;

	return true;
}
Exemplo n.º 4
0
	const OperandREG8 RegisterAllocator::r8(const OperandREF &ref, bool copy)
	{
		OperandREG32 reg = r32(ref, copy);

		// Make sure we only have al, cl, dl or bl
		if(reg.reg >= 4)
		{
			spill(reg);

			// Need to spill one of al, cl, dl or bl
			int candidate = 0;
			unsigned int priority = 0xFFFFFFFF;

			for(int i = 0; i < 4; i++)
			{
				if(GPR[i].priority < priority)
				{
					priority = GPR[i].priority;
					candidate = i;
				}
			}

			spill(OperandREG32(candidate));

			return (OperandREG8)allocate32(candidate, ref, copy, 1);
		}

		return (OperandREG8)reg;
	}
CompressedModuleDescriptor::CompressedModuleDescriptor(const uint8_t * const buffer) : Descriptor(buffer)
{
	ASSERT_MIN_DLEN(5);

	compressionMethod = buffer[2];
	originalSize = r32(&buffer[3]);
}
GroupLinkDescriptor::GroupLinkDescriptor(const uint8_t * const buffer) : Descriptor(buffer)
{
	ASSERT_MIN_DLEN(5);

	position = buffer[2];
	groupId = r32(&buffer[3]);
}
Exemplo n.º 7
0
//--------------------------------------------------------------------------------------------------
void q3Body::SetToAwake( )
{
	if( !(m_flags & eAwake) )
	{
		m_flags |= eAwake;
		m_sleepTime = r32( 0.0 );
	}
}
Exemplo n.º 8
0
//--------------------------------------------------------------------------------------------------
// q3Body
//--------------------------------------------------------------------------------------------------
q3Body::q3Body( const q3BodyDef& def, q3Scene* scene )
{
	m_linearVelocity = def.linearVelocity;
	m_angularVelocity = def.angularVelocity;
	q3Identity( m_force );
	q3Identity( m_torque );
	m_q.Set( q3Normalize( def.axis ), def.angle );
	m_tx.rotation = m_q.ToMat3( );
	m_tx.position = def.position;
	m_sleepTime = r32( 0.0 );
	m_gravityScale = def.gravityScale;
	m_layers = def.layers;
	m_userData = def.userData;
	m_scene = scene;
	m_flags = 0;
	m_linearDamping = def.linearDamping;
	m_angularDamping = def.angularDamping;

	if ( def.bodyType == eDynamicBody )
		m_flags |= q3Body::eDynamic;

	else
	{
		if ( def.bodyType == eStaticBody )
		{
			m_flags |= q3Body::eStatic;
			q3Identity( m_linearVelocity );
			q3Identity( m_angularVelocity );
			q3Identity( m_force );
			q3Identity( m_torque );
		}

		else if ( def.bodyType == eKinematicBody )
			m_flags |= q3Body::eKinematic;
	}

	if ( def.allowSleep )
		m_flags |= eAllowSleep;

	if ( def.awake )
		m_flags |= eAwake;

	if ( def.active )
		m_flags |= eActive;

	if ( def.lockAxisX )
		m_flags |= eLockAxisX;

	if ( def.lockAxisY )
		m_flags |= eLockAxisY;

	if ( def.lockAxisZ )
		m_flags |= eLockAxisZ;

	m_boxes = NULL;
	m_contactList = NULL;
}
Exemplo n.º 9
0
Arquivo: kmem.c Projeto: taysom/tau
unint line32 (addr address)
{
	unint	i;

	for (i = 0; i < U32_LINE; i++, address += sizeof(u32)) {
		printf(Format32[Zerofill], r32(address));
	}
	return U8_LINE;
}
Exemplo n.º 10
0
//--------------------------------------------------------------------------------------------------
void q3Body::SetToSleep( )
{
	m_flags &= ~eAwake;
	m_sleepTime = r32( 0.0 );
	q3Identity( m_linearVelocity );
	q3Identity( m_angularVelocity );
	q3Identity( m_force );
	q3Identity( m_torque );
}
Exemplo n.º 11
0
void check_ports(void)
{
 unsigned long control,irq_status,status1,status2;

 control   =r32(HcControl);
 irq_status=r32(HcIntStatus);
 status1   =r32(HcRhP1);
 status2   =r32(HcRhP2);

  if((status1&0x00000001)==1)
  {
   w32(HcRhP1,0x00000102);
   if((status1&0x00000200)!=0)
   {
	printf("\nLS Device connected to Port 1     ");
	set_port_speed(1,1);
   }
   else
   {
	printf("\nFS Device connected to Port 1     ");
 	set_port_speed(1,0);
   }
  }

  if((status2&0x00000001)==1)
  {
   w32(HcRhP2,0x00000102);
   if((status2&0x00000200)!=0)
   {
	printf("\nLS Device connected to Port 2     ");
	set_port_speed(2,1);
   }
   else
   {
	printf("\nFS Device connected to Port 2     ");
	set_port_speed(2,0);
   }
  }

 printf("\nControl    Register = %8lX.",control);
 printf("\nInt Status Register = %8lX.",irq_status);
 printf("\nP1  Status Register = %8lX.",status1);
 printf("\nP2  Status Register = %8lX.",status2);
}
Exemplo n.º 12
0
QSharedPointer<Handle> BIF::get(quint16 tileset, quint16 index) {
  handle->seek(0);
  QString key = handle->r4();
  if (key != "BIFF" && key != "BIFC")
    throw FileError("Not a BIF file");
  bool compressed = key == "BIFC";

  QString ver = handle->r4();
  if (ver != "V1  " && ver != "V1.0")
    throw FileError("Invalid BIF version");

  if (!tileset) {
    if (compressed) {
      auto h = getBlock(16, 4);
      quint32 fileOffs = h->r32() + index * 16;
      h = getBlock(fileOffs, 16);
      h->skip(4); //locator
      quint32 offset = h->r32();
      quint32 size = h->r32();
      return getBlock(offset, size);
    } else {
      handle->skip(8); //num files and num tiles
      quint32 fileOffs = handle->r32() + index * 16;
      handle->seek(fileOffs);
      handle->skip(4); //locator
      quint32 offset = handle->r32();
      quint32 size = handle->r32();
      return QSharedPointer<Handle>(new Handle(handle, offset, size));
    }
  } else {
    if (compressed) {
      auto h = getBlock(8, 4);
      quint32 numFiles = h->r32();
      h = getBlock(16, 4);
      quint32 fileOffs = h->r32() + numFiles * 16 + tileset * 20;
      h = getBlock(fileOffs, 20);
      h->skip(4); //locator
      quint32 offset = h->r32();
      quint32 numTiles = h->r32();
      quint32 size = h->r32();
      return getBlock(offset, size * numTiles);
    } else {
      quint32 numFiles = handle->r32();
      handle->skip(4); //num tiles
      quint32 fileOffs = handle->r32() + numFiles * 16 + tileset * 20;
      handle->seek(fileOffs);
      handle->skip(4); //locator
      quint32 offset = handle->r32();
      quint32 numTiles = handle->r32();
      quint32 size = handle->r32();
      return QSharedPointer<Handle>(new Handle(handle, offset, size * numTiles));
    }
  }
}
Exemplo n.º 13
0
	TestRegisterAllocator() : CodeGenerator(false)
	{
		x1 = 1;
		x2 = 2;
		x3 = 3;
		x4 = 4;
		x5 = 5;
		x6 = 6;
		x7 = 7;
		x8 = 8;
		x9 = 9;

		prologue(0);

		Int t1;
		Int t2;
		Int t3;
		Int t4;
		Int t5;
		Int t6;
		Int t7;
		Int t8;
		Int t9;

		mov(t1, r32(&x1));
		mov(t2, r32(&x2));
		mov(t3, r32(&x3));
		mov(t4, r32(&x4));
		mov(t5, r32(&x5));
		mov(t6, r32(&x6));
		mov(t7, r32(&x7));
		mov(t8, r32(&x8));
		mov(t9, r32(&x9));

		mov(dword_ptr [&x1], t9);
		mov(dword_ptr [&x2], t8);
		mov(dword_ptr [&x3], t7);
		mov(dword_ptr [&x4], t6);
		mov(dword_ptr [&x5], t5);
		mov(dword_ptr [&x6], t4);
		mov(dword_ptr [&x7], t3);
		mov(dword_ptr [&x8], t2);
		mov(dword_ptr [&x9], t1);

		epilogue();
	}
Exemplo n.º 14
0
double collinearityy(double yo, double c, double XA, double XO, double YA,
					double YO, double ZA, double ZO, double omega,
					double phi, double kapa)
{
	double o = gtr(omega);
	double f = gtr(phi);
	double k = gtr(kapa);
	
	double ya;
	ya = yo - c*((XA-XO)*r12(o,f,k) + (YA-YO)*r22(o,f,k) +(ZA-ZO)*r32(o,f))
		/((XA-XO)*r13(o,f,k) + (YA-YO)*r23(o, f, k) +(ZA-ZO)*r33(o, f));
	return (ya);
}
Exemplo n.º 15
0
//--------------------------------------------------------------------------------------------------
void q3Body::SetAngularVelocity( const q3Vec3 v )
{
	// Velocity of static bodies cannot be adjusted
	if ( m_flags & eStatic )
		assert( false );

	if ( q3Dot( v, v ) > r32( 0.0 ) )
	{
		SetToAwake( );
	}

	m_angularVelocity = v;
}
Exemplo n.º 16
0
void b3Polyhedron::ComputeMass(b3MassData* massData, r32 density) const {
	b3Assert(m_hull);
	b3Assert(m_hull->vertexCount >= 3);

	// We will pick the block local inertia tensor.

	b3AABB aabb;
	aabb.ComputeAabbFromPointArray(m_hull->vertices, m_hull->vertexCount);

	r32 w = aabb.Width();
	r32 h = aabb.Height();
	r32 d = aabb.Depth();

	r32 volume = w * h * d;
	massData->mass = volume * density;

	r32 ww = w * w;
	r32 hh = h * h;
	r32 dd = d * d;

	r32 invTwelve = B3_ONE / r32(12.0);
	r32 invTwelveMass = invTwelve * massData->mass;

	// Inertia tensor in local space.
	massData->I.SetZero();
	massData->I.x.x = invTwelveMass * (hh + dd);
	massData->I.y.y = invTwelveMass * (ww + dd);
	massData->I.z.z = invTwelveMass * (ww + hh);

	// Compute average particle positions.
	massData->center.SetZero();
	for ( u32 i = 0; i < m_hull->vertexCount; ++i ) {
		massData->center += m_hull->vertices[i];
	}
	massData->center *= B3_ONE / r32(m_hull->vertexCount);
}
Exemplo n.º 17
0
//--------------------------------------------------------------------------------------------------
void q3Box::ComputeMass( q3MassData* md ) const
{
	// Calculate inertia tensor
	r32 ex2 = r32( 4.0 ) * e.x * e.x;
	r32 ey2 = r32( 4.0 ) * e.y * e.y;
	r32 ez2 = r32( 4.0 ) * e.z * e.z;
	r32 mass = r32( 8.0 ) * e.x * e.y * e.z * density;
	r32 x = r32( 1.0 / 12.0 ) * mass * (ey2 + ez2);
	r32 y = r32( 1.0 / 12.0 ) * mass * (ex2 + ez2);
	r32 z = r32( 1.0 / 12.0 ) * mass * (ex2 + ey2);
	q3Mat3 I = q3Diagonal( x, y, z );

	// Transform tensor to local space
	I = local.rotation * I * q3Transpose( local.rotation );
	q3Mat3 identity;
	q3Identity( identity );
	I += (identity * q3Dot( local.position, local.position ) - q3OuterProduct( local.position, local.position )) * mass;

	md->center = local.position;
	md->inertia = I;
	md->mass = mass;
}
Exemplo n.º 18
0
void b3Island::Solve(const b3Vec3& gravityDir) {
	r32 h = dt;
	b3Vec3 gravityForce = B3_GRAVITY_ACC * gravityDir;

	// Integrate velocities.
	for (u32 i = 0; i < bodyCount; ++i) {
		b3Body* b = bodies[i];

		b3Vec3 v = b->m_linearVelocity;
		b3Vec3 w = b->m_angularVelocity;
		b3Vec3 x = b->m_worldCenter;
		b3Quaternion q = b->m_orientation;

		if (b->m_type == e_dynamicBody) {
			// Use semi-implitic Euler.
			b3Vec3 force = b->m_gravityScale * gravityForce + b->m_force;
			v += (h * b->m_invMass) * force;
			w += h * (b->m_invWorldInertia * b->m_torque);

			// References: Box2D.
			// Apply damping.
			// ODE: dv/dt + c * v = 0
			// Solution: v(t) = v0 * exp(-c * t)
			// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
			// v2 = exp(-c * dt) * v1
			// Pade approximation:
			// v2 = v1 * 1 / (1 + c * dt)
			v *= B3_ONE / (B3_ONE + h * r32(0.1));
			w *= B3_ONE / (B3_ONE + h * r32(0.1));
		}

		velocities[i].v = v;
		velocities[i].w = w;
		positions[i].x = x;
		positions[i].q = q;
	}

	b3JointSolverDef jointSolverDef;
	jointSolverDef.dt = h;
	jointSolverDef.joints = joints;
	jointSolverDef.count = jointCount;
	jointSolverDef.positions = positions;
	jointSolverDef.velocities = velocities;

	b3JointSolver jointSolver(&jointSolverDef);
	jointSolver.InitializeVelocityConstraints();

	b3ContactSolverDef contactSolverDef;
	contactSolverDef.dt = h;
	contactSolverDef.contacts = contacts;
	contactSolverDef.count = contactCount;
	contactSolverDef.positions = positions;
	contactSolverDef.velocities = velocities;
	contactSolverDef.allocator = allocator;

	b3ContactSolver contactSolver(&contactSolverDef);
	contactSolver.InitializeVelocityConstraints();
	
	jointSolver.WarmStart();
	contactSolver.WarmStart();
	
	// Solve velocity constraints.
	for (u32 i = 0; i < velocityIterations; ++i) {
		jointSolver.SolveVelocityConstraints();
		contactSolver.SolveVelocityConstraints();
	}

	contactSolver.StoreImpulses();

	for (u32 i = 0; i < bodyCount; ++i) {
		b3Body* b = bodies[i];
		if (b->m_type == e_staticBody) {
			continue;
		}

		b3Vec3 x = positions[i].x;
		b3Quaternion q1 = positions[i].q;
		b3Vec3 v = velocities[i].v;
		b3Vec3 w = velocities[i].w;
		
		x += h * v;
		
		b3Quaternion q2 = Integrate(q1, w, h);

		positions[i].x = x;
		positions[i].q = q2;
		velocities[i].v = v;
		velocities[i].w = w;
	}

	for (u32 i = 0; i < bodyCount; ++i) {
		b3Body* b = bodies[i];
		if (b->m_type == e_staticBody) {
			continue;
		}

		b->m_worldCenter = positions[i].x;
		b->m_orientation = positions[i].q;
		b->m_linearVelocity = velocities[i].v;
		b->m_angularVelocity = velocities[i].w;
	}

	if (allowSleep) {
		r32 minSleepTime = B3_MAX_FLOAT;
		for (u32 i = 0; i < bodyCount; ++i) {
			b3Body* b = bodies[i];
			if (b->m_type == e_staticBody) {
				continue;
			}

			// Compute the linear and angular speed of the body.
			const r32 sqrLinVel = b3LenSq(b->m_linearVelocity);
			const r32 sqrAngVel = b3LenSq(b->m_angularVelocity);

			if (sqrLinVel > B3_SLEEP_LINEAR_TOL || sqrAngVel > B3_SLEEP_ANGULAR_TOL) {
				minSleepTime = B3_ZERO;
				b->m_sleepTime = B3_ZERO;
			}
			else {
				b->m_sleepTime += h;
				minSleepTime = b3Min(minSleepTime, b->m_sleepTime);
			}
		}

		// Put the island to sleep so long as the minimum found sleep time
		// is below the threshold. 
		if (minSleepTime >= B3_TIME_TO_SLEEP) {
			for (u32 i = 0; i < bodyCount; ++i) {
				bodies[i]->SetAwake(false);
			}
		}
	}
}
Exemplo n.º 19
0
//--------------------------------------------------------------------------------------------------
void q3ContactManager::TestCollisions( void )
{
	q3ContactConstraint* constraint = m_contactList;

	while( constraint )
	{
		q3Box *A = constraint->A;
		q3Box *B = constraint->B;
		q3Body *bodyA = A->body;
		q3Body *bodyB = B->body;

		constraint->m_flags &= ~q3ContactConstraint::eIsland;

		if( !bodyA->IsAwake( ) && !bodyB->IsAwake( ) )
		{
			constraint = constraint->next;
			continue;
		}

		if ( !bodyA->CanCollide( bodyB ) )
		{
			q3ContactConstraint* next = constraint->next;
			RemoveContact( constraint );
			constraint = next;
			continue;
		}

		// Check if contact should persist
		if ( !m_broadphase.TestOverlap( A->broadPhaseIndex, B->broadPhaseIndex ) )
		{
			q3ContactConstraint* next = constraint->next;
			RemoveContact( constraint );
			constraint = next;
			continue;
		}
		q3Manifold* manifold = &constraint->manifold;
		q3Manifold oldManifold = constraint->manifold;
		q3Vec3 ot0 = oldManifold.tangentVectors[ 0 ];
		q3Vec3 ot1 = oldManifold.tangentVectors[ 1 ];
		constraint->SolveCollision( );
		q3ComputeBasis( manifold->normal, manifold->tangentVectors, manifold->tangentVectors + 1 );

		for ( i32 i = 0; i < manifold->contactCount; ++i )
		{
			q3Contact *c = manifold->contacts + i;
			c->tangentImpulse[ 0 ] = c->tangentImpulse[ 1 ] = c->normalImpulse = r32( 0.0 );
			u8 oldWarmStart = c->warmStarted;
			c->warmStarted = u8( 0 );

			for ( i32 j = 0; j < oldManifold.contactCount; ++j )
			{
				q3Contact *oc = oldManifold.contacts + j;
				if ( c->fp.key == oc->fp.key )
				{
					c->normalImpulse = oc->normalImpulse;

					// Attempt to re-project old friction solutions
					q3Vec3 friction = ot0 * oc->tangentImpulse[ 0 ] + ot1 * oc->tangentImpulse[ 1 ];
					c->tangentImpulse[ 0 ] = q3Dot( friction, manifold->tangentVectors[ 0 ] );
					c->tangentImpulse[ 1 ] = q3Dot( friction, manifold->tangentVectors[ 1 ] );
					c->warmStarted = q3Max( oldWarmStart, u8( oldWarmStart + 1 ) );
					break;
				}
			}
		}

		if ( m_contactListener )
		{
			i32 now_colliding = constraint->m_flags & q3ContactConstraint::eColliding;
			i32 was_colliding = constraint->m_flags & q3ContactConstraint::eWasColliding;

			if ( now_colliding && !was_colliding )
				m_contactListener->BeginContact( constraint );

			else if ( !now_colliding && was_colliding )
				m_contactListener->EndContact( constraint );
		}

		constraint = constraint->next;
	}
}
Exemplo n.º 20
0
	StressTest(int seed, int tests, int level, bool copyProp, bool loadElim, bool spillElim) : CodeGenerator(false)
	{
		if(copyProp) enableCopyPropagation(); else disableCopyPropagation();
		if(loadElim) enableLoadElimination(); else disableLoadElimination();
		if(spillElim) enableSpillElimination(); else disableSpillElimination();
	#if 0
		Int a;
		Int b;
		Int c;

		pushad();
		prologue(1024);
		freeAll();

		for(int i = 0; i < 3; i++)
		{
			add(r32(&x[2 * i + 0]), r32(&x[2 * i + 1]));
		}

		nop();

		a = (a + b) * c;

		nop();

		for(int i = 2; i >= 1; i--)
		{
			add(r32(&x[2 * i + 0]), r32(&x[2 * i + 1]));
		}

		nop();
		spillAll();
		epilogue();
		popad();
		ret();

		return;
	#else
		// -------------------------------------------------

		srand(seed);

		for(int i = 0; i < 16; i++)
		{
			w[i] = rand();
		}

		for(int i = 0; i < 16; i++)
		{
			x[i] = w[i];
		}

		if(level & 0x01) for(int i = 0; i < tests; i++)
		{
			x[q()] = x[q()];

			if(i % 3 == 0 && rand() < RAND_MAX / 2)
			{
				int a = q(); int b = q();
				x[b] += x[a];
			}
		}

		if(level & 0x02) for(int i = 0; i < tests; i++)
		{
			int a = q(); int b = q();
			x[b] += x[a];

			if(i % 3 == 0 && rand() < RAND_MAX / 2)
			{
				x[q()] = x[q()];
			}
		}

		if(level & 0x04) for(int i = 0; i < tests; i++)
		{
			int a = q(); int b = q();
			x[b] += x[a];

			if(i % 3 == 0 && rand() < RAND_MAX / 2)
			{
				x[q()];   // spill(reg)
			}
		}

		if(level & 0x08) for(int i = 0; i < tests; i++)
		{
			if(q() < q() / 2) {int a = q(); int b = q(); x[b] += x[a];}   // add(r(), r());
			if(q() < q() / 2) {x[q()] = x[q()];}   // mov(r(), r());
			if(q() < q() / 2) q();   // spill(r());
		}

		for(int i = 0; i < 16; i++)
		{
			y[i] = x[i];
		}

		// -------------------------------------------------

		srand(seed);

		for(int i = 0; i < 16; i++)
		{
			w[i] = rand();
		}

		pushad();

		for(int i = 0; i < 16; i++)
		{
			mov(eax, dword_ptr [&w[i]]);
			mov(dword_ptr [&x[i]], eax);
		}

		freeAll();

		nop();

		if(level & 0x01) for(int i = 0; i < tests; i++)
		{
			mov(r(), r());

			if(i % 3 == 0 && rand() < RAND_MAX / 2)
			{
				add(r(), r());
			}
		}

		if(level & 0x02) for(int i = 0; i < tests; i++)
		{
			add(r(), r());

			if(i % 3 == 0 && rand() < RAND_MAX / 2)
			{	
				mov(r(), r());
			}
		}

		if(level & 0x04) for(int i = 0; i < tests; i++)
		{
			add(r(), r());

			if(i % 3 == 0 && rand() < RAND_MAX / 2)
			{	
				spill(r());
			}
		}

		if(level & 0x08) for(int i = 0; i < tests; i++)
		{
			if(q() < q() / 2) add(r(), r());
			if(q() < q() / 2) mov(r(), r());
			if(q() < q() / 2) spill(r());
		}

		nop();

		spillAll();

		nop();

		for(int i = 0; i < 16; i++)
		{
			mov(eax, dword_ptr [&x[i]]);
			mov(dword_ptr [&z[i]], eax);
		}

		popad();
		ret();
	#endif
	}
Exemplo n.º 21
0
quint64 TagDataStream::r64() {
  quint64 r = (quint64)r32() << 32;
  r |= r32();
  return r;
}
Exemplo n.º 22
0
	const SoftWire::OperandREG32 r()
	{
		return r32(&x[q()]);
	}
Exemplo n.º 23
0
quint64 Handle::r64() {
  quint64 r = r32();
  r |= ((quint64)r32()) << 32;
  return r;
}
Exemplo n.º 24
0
void port_monitor(void)
{
 unsigned long control,irq_status,status1,status2;
 unsigned int start_y=10;
 unsigned int user_in;

// clrscr();

 set_operational();

 w32(HcRhP1,0x00000102);
 w32(HcRhP2,0x00000102);
 w32(HcRhA,0x20000002);
 w32(HcRhB,0x00000000);
 w32(HcRhStatus,0x00010000);	//set Global Power

 do
 {
  control   =r32(HcControl);
  irq_status=r32(HcIntStatus);
  status1   =r32(HcRhP1);
  status2   =r32(HcRhP2);

  if((status1&0x00000001)==1)
  {
//   gotoxy(20,start_y+6);
   w32(HcRhP1,0x00000102);
   if((status1&0x00000200)!=0)
   {
	printf("LS Device connected to Port 1");
	set_port_speed(1,1);
   }
   else
   {
	printf("FS Device connected to Port 1");
	set_port_speed(1,0);
   }
  }

  else
  {
//   gotoxy(20,start_y+6);
   printf("                                      ");
  }

  if((status2&0x00000001)==1)
  {
//   gotoxy(20,start_y+7);
   w32(HcRhP2,0x00000102);
   if((status2&0x00000200)!=0)
   {
	printf("LS Device connected to Port 2");
	set_port_speed(2,1);
   }
   else
   {
	printf("FS Device connected to Port 2");
	set_port_speed(2,0);
   }
  }

  else
  {
//   gotoxy(20,start_y+7);
   printf("                                      ");
  }

//  gotoxy(20,start_y);
  printf("Port Status Monitor");
 // gotoxy(20,start_y-1);
  printf("OTGStatus  Register = %8X.\n",r16(0x67));
 // gotoxy(20,start_y+1);
  printf("OTGControl Register = %8X.\n",r16(0x62));
//  gotoxy(20,start_y+2);
  printf("Control    Register = %8lX.\n",control);
//  gotoxy(20,start_y+3);
  printf("Int Status Register = %8lX.\n",irq_status);
//  gotoxy(20,start_y+4);
  printf("P1  Status Register = %8lX.\n",status1);
//  gotoxy(20,start_y+5);
  printf("P2  Status Register = %8lX.\n",status2);
//  gotoxy(20,start_y+8);
  printf("Press '1' to go back to main menu\n");

  user_in='1';//read_key(0);
 }
 while(user_in!='1');
}
Exemplo n.º 25
0
void print_header( std::string const& model, std::string const& name, color::rgb<double>  const& r )
 {
  color_name<uint8_t>      i8   ( r );
  color_name<uint16_t>     i16  ( r );
  color_name<uint32_t>     i32  ( r );
  color_name<uint64_t>     i64  ( r );
  color_name<float>        f    ( r );
  color_name<double>       d    ( r );
  color_name<long double>  ld   ( r );

  color::rgb<uint8_t> r32( r );

  std::stringstream ss;

  ss << "#ifndef color_"<< model <<"_make_" << name                                                       << std::endl;
  ss << "#define color_"<< model <<"_make_" << name                                                       << std::endl;
  ss                                                                                                      << std::endl;
  ss << "// ::color::make::" << name << "( c )"                                                           << std::endl;
  ss                                                                                                      << std::endl;
  ss << " namespace color"                                                                                << std::endl;
  ss << "  {"                                                                                             << std::endl;
  ss << "   namespace make"                                                                               << std::endl;
  ss << "    { //RGB equivalents: ";
  ss << "std::array<double,3>( { "<< d[0]<<", "<< d[1]<<", "<< d[2]<<" } )";
  ss << " - ";
  ss << "rgb(" << std::setbase(10) <<(unsigned)r32[0] << "," << (unsigned)r32[1] << "," << (unsigned)r32[2] << ")" ;
  ss << " - ";
  ss << "#" << std::setbase(16) <<  std::setw(2) <<  std::setfill('0') << (unsigned)r32[0]
            << std::setbase(16) <<  std::setw(2) <<  std::setfill('0') << (unsigned)r32[1]
            << std::setbase(16) <<  std::setw(2) <<  std::setfill('0') << (unsigned)r32[2] ;
  ss << std::endl;
  ss                                                                                                      << std::endl;

  ss << "      inline"                                                                                    << std::endl;
  ss << "      void " << name << "( ::color::_internal::model< ::color::category::"<< model <<"_uint8 > & color_parameter )"    << std::endl;
  ss << "       {"                                                                                        << std::endl;
  ss << "        color_parameter.container() = std::array< std::uint8_t, " << i8.size() << " >( { " 
     << "0x" << std::setbase(16) <<  std::setw(2) <<  std::setfill('0') <<  (unsigned)i8[0] << ", " 
     << "0x" << std::setbase(16) <<  std::setw(2) <<  std::setfill('0') <<  (unsigned)i8[1] << ", " 
     << "0x" << std::setbase(16) <<  std::setw(2) <<  std::setfill('0') <<  (unsigned)i8[2];
  if( 4 == i8.size() ){ ss << ", 0x" << std::setbase(16) <<  std::setw(2) <<  std::setfill('0') <<  (unsigned)i8[3];  }
  ss  << " } );" << std::endl;
  ss << "       }"                                                                                        << std::endl;
  ss                                                                                                      << std::endl;
  
  
  ss << "      inline"                                                                                    << std::endl;
  ss << "      void " << name << "( ::color::_internal::model< ::color::category::"<< model <<"_uint16 > & color_parameter )"    << std::endl;
  ss << "       {"                                                                                        << std::endl;
  ss << "        color_parameter.container() = std::array< std::uint16_t, " << i16.size() << " >( { "  
   << "0x" << std::setbase(16) <<  std::setw(4) <<  std::setfill('0') << i16[0] << ", " 
   << "0x" << std::setbase(16) <<  std::setw(4) <<  std::setfill('0') << i16[1] << ", " 
   << "0x" << std::setbase(16) <<  std::setw(4) <<  std::setfill('0') << i16[2];
  if( 4 == i16.size() ){ ss << ", 0x" << std::setbase(16) <<  std::setw(4) <<  std::setfill('0') <<  i16[3];  }
  ss  << " } );" << std::endl;
  ss << "       }"                                                                                        << std::endl;
  ss                                                                                                      << std::endl;

  ss << "      inline"                                                                                    << std::endl;
  ss << "      void " << name << "( ::color::_internal::model< ::color::category::"<< model <<"_uint32 > & color_parameter )"    << std::endl;
  ss << "       {"                                                                                        << std::endl;
  ss << "        color_parameter.container() = std::array< std::uint32_t, " << f.size() << " >( { " 
     << "0x" << std::setbase(16) <<  std::setw(8) <<  std::setfill('0') << i32[0] << ", " 
     << "0x" << std::setbase(16) <<  std::setw(8) <<  std::setfill('0') << i32[1] << ", " 
     << "0x" << std::setbase(16) <<  std::setw(8) <<  std::setfill('0') << i32[2];
  if( 4 == i32.size() ){ ss << ", 0x" << std::setbase(16) <<  std::setw(8) <<  std::setfill('0') <<  i32[3];  }
  ss  << " } );" << std::endl;
  ss << "       }"                                                                                        << std::endl;
  ss                                                                                                      << std::endl;

  ss << "      inline"                                                                                    << std::endl;
  ss << "      void " << name << "( ::color::_internal::model< ::color::category::"<< model <<"_uint64 > & color_parameter )"    << std::endl;
  ss << "       {"                                                                                        << std::endl;
  ss << "        color_parameter.container() = std::array< std::uint64_t, " << i64.size() << " >( { " 
     << "0x" << std::setbase(16) <<  std::setw(16) <<  std::setfill('0') << i64[0] << "ull, " 
     << "0x" << std::setbase(16) <<  std::setw(16) <<  std::setfill('0') << i64[1] << "ull, " 
     << "0x" << std::setbase(16) <<  std::setw(16) <<  std::setfill('0') << i64[2] << "ull" ;
  if( 4 == i64.size() ){ ss << ", 0x" << std::setbase(16) <<  std::setw(16) <<  std::setfill('0') << i64[3] << "ull";  }
  ss  << " } );" << std::endl;
  ss << "       }"                                                                                        << std::endl;
  ss                                                                                                      << std::endl;

  ss << "      inline"                                                                                    << std::endl;
  ss << "      void " << name << "( ::color::_internal::model< ::color::category::"<< model <<"_float > & color_parameter )"    << std::endl;
  ss << "       {"                                                                                        << std::endl;
  ss << "        color_parameter.container() = std::array<float," << f.size() << ">( { " << f[0] << ", " << f[1] << ", " << f[2];
  if( 4 == f.size() ){ ss << ", " << f[3];  }
  ss  << " } );" << std::endl;
  ss << "       }"                                                                                        << std::endl;
  ss                                                                                                      << std::endl;

  ss << "      inline"                                                                                    << std::endl;
  ss << "      void " << name << "( ::color::_internal::model< ::color::category::"<< model <<"_double> & color_parameter )"    << std::endl;
  ss << "       {"                                                                                        << std::endl;
  ss << "        color_parameter.container() = std::array<double," << d.size() << ">( { " << d[0] << ", " << d[1] << ", " << d[2];
  if( 4 == d.size() ){ ss << ", " << d[3];  }
  ss  << " } );" << std::endl;
  ss << "       }"                                                                                        << std::endl;
  ss                                                                                                      << std::endl;
  ss << "      inline"                                                                                    << std::endl;
  ss << "      void " << name << "( ::color::_internal::model< ::color::category::"<< model <<"_ldouble> & color_parameter )"   << std::endl;
  ss << "       {"                                                                                        << std::endl;
  ss << "        color_parameter.container() = std::array<long double," << ld.size() << ">( { " << ld[0] << ", " << ld[1] << ", " << ld[2];
  if( 4 == ld.size() ){ ss << ", " << ld[3];}
  ss  << " } );" << std::endl;
  ss << "       }"                                                                                        << std::endl;
  ss                                                                                                      << std::endl;
  ss << "    }"                                                                                           << std::endl;
  ss << "  }"                                                                                             << std::endl;
  ss                                                                                                      << std::endl;
  ss << "#endif"                                                                                          << std::endl;

  //std::cout << ss.str();
  //std::cout << "-------" << std:: endl;

   {
    std::ofstream ofs( ( "./gen-"+ model +"/"+name + ".hpp" ). c_str() );
    //std::ofstream ofs( ( "../../../src/color/"+ model +"/make/"+name + ".hpp" ). c_str() );
    ofs <<  ss.str();
   }

  }
Exemplo n.º 26
0
unsigned int assign_address(unsigned int addr1, unsigned int addr2, int mode)
{
 unsigned long rhp1,rhp2;
 unsigned int status;
 unsigned int ccode=0;

 rhp1=r32(HcRhP1);
 rhp2=r32(HcRhP2);

 enable_port();

// gotoxy(1,5);

 if(mode==1)
 {
  printf("\nRhP1 = %8lX  RhP2 = %8lX",rhp1,rhp2);
  printf("\nPort1Speed = %2d",get_port_speed(1));
  printf("\nPort2Speed = %2d",get_port_speed(2));
 }

 if(  ((rhp1&0x01)==1)  &&  ((rhp2&0x01)==1)  )
 {
  if(mode==1)printf("\nBoth ports has USB device connected.");

  w32(HcRhP1,0x00000010); //Resets port 1
//  delay(500);
  w32(HcRhP2,0x00000010); //Resets port 2

  ccode=set_address(0,1,1);
  status=0x0100|(ccode<<12);

  enable_port();

//  delay(200);
  ccode=set_address(0,2,2);

  status=0x0001|(ccode<<4);
 }

 else if(  ((rhp1&0x01)==1)  &&  ((rhp2&0x01)==0)  )
 {
  if(mode==1)printf("\nPort 1 has USB device connected, assigning address 1...");
   ccode=set_address(0,1,1);

  status=0x0100|(ccode<<12);
 }

 else if(  ((rhp1&0x01)==0)  &&  ((rhp2&0x01)==1)  )
 {
  if(mode==1)printf("\nPort 2 has USB device connected, assigning address 2...");
   ccode=set_address(0,2,2);

  status=0x0001|(ccode<<4);
 }

 else
 {
  if(mode==1)printf("\nNo device connected to ISP1362, aborting enumeration...");

  status=0x0000;
 }

 return(status);
}
Exemplo n.º 27
0
/**
 * Get the Wilson's B-matrix
 */
void getBMatrix(Real** cartCoords, int numCartesians,
		bondCoord** bonds, int numBonds,
		angleCoord** angles, int numAngles,
		dihedralCoord** dihedrals, int numDihedrals,
		improperCoord** impropers, int numImpropers,
		Matrix& B) {
#ifdef DEBUG
  cout << "\n\ngetBMatrix - Constructing B Matrix\n";
#endif

  // Constructing B Matrix
  //   follows method in chapter 4 of Molecular Vibrations by Wilson, Decius, and Cross

#ifdef DEBUG  
  int numInternals = numBonds + numAngles + numDihedrals + numImpropers;
  cout << "numBonds: " << numBonds << "\n";
  cout << "numAngles: " << numAngles << "\n";
  cout << "numDihedrals: " << numDihedrals << "\n";
  cout << "numImpropers: " << numImpropers << "\n";
  cout << "numInternals: " << numInternals << "\n";
#endif

  // Load Data
  B = 0.0;
  int i = 0;
  int j = 0;
  int index1 = 0;
  int index2 = 0;
  RowVector tempCoord1(3);
  RowVector tempCoord2(3);
  Real norm = 0.0;
  
  // Bonds
  for (i=0; i<numBonds; i++) {
    index1 = bonds[i]->x1;
    index2 = bonds[i]->x2;
    //norm = bonds[i].val;   // Could calculate this, like below.
#ifdef DEBUG
    cout << "index1=" << index1 << "index2=" << index2 << "\n";
#endif
    for (j=0; j<3; j++) {
      tempCoord1(j+1) = cartCoords[index1][j];
      tempCoord2(j+1) = cartCoords[index2][j];
    }
    tempCoord1 << tempCoord1 - tempCoord2;
    norm = tempCoord1.NormFrobenius();   // XXX - don't delete
    if (norm > 0.0) {
      tempCoord1 << tempCoord1 / norm;
    }
    for (j=1; j<=3; j++) {
      B(i+1,((index1)*3)+j) =  tempCoord1(j);
      B(i+1,((index2)*3)+j) = -tempCoord1(j);
    }
  }
  
#ifdef DEBUG
  cout << "after bonds\n";
  cout << "B:\n";
  cout << setw(9) << setprecision(3) << (B);
  cout << "\n\n";
#endif

  // Angles
  int index3 = 0;
  RowVector tempCoord3(3);
  RowVector tempCoord4(3);
  RowVector tempCoord5(3);
  RowVector r21(3);   // Vector from 2nd to 1st point
  RowVector r23(3);   // Vector from 2nd to 3rd point
  RowVector e21(3);   // Unit vector from 2nd to 1st point
  RowVector e23(3);   // Unit vector from 2nd to 3rd point
  Real norm21;        // Norm of r21
  Real norm23;        // Norm of r23
  Real angle = 0.0;   // Angle in radians
  Real cosAngle123 = 0.0;
  Real sinAngle123 = 0.0;
  //Real pi = 3.14159265;
  Real scaleFactor = 0.529178;   // Scaling factor (0.529178)
  for (i=0; i<numAngles; i++) {
    index1 = angles[i]->x1;
    index2 = angles[i]->x2;
    index3 = angles[i]->x3;
    //angle = angles[i].val * (pi/180.0);   // Convert to radians.
    for (j=0; j<3; j++) {
      tempCoord1(j+1) = cartCoords[index1][j];
      tempCoord2(j+1) = cartCoords[index2][j];
      tempCoord3(j+1) = cartCoords[index3][j];
    }
    r21 << tempCoord1 - tempCoord2;
    r23 << tempCoord3 - tempCoord2;
    norm21 = r21.NormFrobenius();
    norm23 = r23.NormFrobenius();
    e21 << r21;
    if (norm21 > 0.0) {
      e21 << e21 / norm21;
    }
    e23 << r23;
    if (norm23 > 0.0) {
      e23 << e23 / norm23;
    }
    angle = acos(DotProduct(r21,r23) / (norm21 * norm23));
    cosAngle123 = DotProduct(r21,r23) / (norm21 * norm23);
    sinAngle123 = sqrt(1 - (cosAngle123 * cosAngle123));
    
#ifdef DEBUG
    cout << "r21: " << (r21) << "\n";
    cout << "r23: " << (r23) << "\n";
    cout << "norm21: " << norm21 << ", norm23: " << norm23 << "\n\n";
    cout << "e21: " << (e21) << "\n";
    cout << "e23: " << (e23) << "\n";
    cout << "cos(" << angle << "): " << cos(angle) << "\n";
    cout << "sin(" << angle << "): " << sin(angle) << "\n";
    cout << "angle: " << acos(DotProduct(r21,r23) / (norm21 * norm23)) << "\n";
    cout << "cosAngle123: " << cosAngle123 << "\n";
    cout << "sinAngle123: " << sinAngle123 << "\n";
#endif    

    // First elements of coordinate triples
    tempCoord4 << ((cosAngle123 * e21) - e23);
    tempCoord4 << (tempCoord4 * scaleFactor) / (norm21 * sinAngle123);
    for (j=1; j<=3; j++) {
      B(i+numBonds+1,((index1)*3)+j) = tempCoord4(j);
    }
    // Third elements of coordinate triples
    tempCoord5 << ((cosAngle123 * e23) - e21);
    tempCoord5 << (tempCoord5 * scaleFactor) / (norm23 * sinAngle123);
    for (j=1; j<=3; j++) {
      B(i+numBonds+1,((index3)*3)+j) = tempCoord5(j);
    }
    // Second (middle) elements of coordinate triples (depends on 1st and 3rd)
    tempCoord4 << -tempCoord4 - tempCoord5;
    for (j=1; j<=3; j++) {
      B(i+numBonds+1,((index2)*3)+j) = tempCoord4(j);
    }
  }
  
#ifdef DEBUG
  cout << "after angles\n";
  cout << "B:\n";
  cout << setw(9) << setprecision(3) << (B);
  cout << "\n\n";
#endif  

  // Dihedrals
  RowVector r12(3);   // Vector from 1st to 2nd point
  RowVector r32(3);   // Vector from 3rd to 2nd point
  RowVector r34(3);   // Vector from 3rd to 2nd point
  RowVector r43(3);   // Vector from 4th to 3rd point
  RowVector e12(3);   // Unit vector from 1st to 2nd point
  RowVector e32(3);   // Unit vector from 3rd to 2nd point
  RowVector e34(3);   // Unit vector from 3rd to 2nd point
  RowVector e43(3);   // Unit vector from 4th to 3rd point
  Real norm12;        // Norm of r12
  Real norm32;        // Norm of r32
  Real norm34;        // Norm of r34
  Real norm43;        // Norm of r43
  RowVector cross1223(3);   // Cross product of e12 and e23
  RowVector cross4332(3);   // Cross product of e43 and e32
  Real angle123 = 0.0;   // Angle in radians
  Real angle234 = 0.0;   // Angle in radians
  Real cosAngle234 = 0.0;
  Real sinAngle234 = 0.0;
  scaleFactor = 0.529178;   // Scaling factor (0.529178)
  int index4 = 0;
  RowVector tempCoord6(3);
  for (i=0; i<numDihedrals; i++) {
    index1 = dihedrals[i]->x1;
    index2 = dihedrals[i]->x2;
    index3 = dihedrals[i]->x3;
    index4 = dihedrals[i]->x4;
    for (j=0; j<3; j++) {
      tempCoord1(j+1) = cartCoords[index1][j];
      tempCoord2(j+1) = cartCoords[index2][j];
      tempCoord3(j+1) = cartCoords[index3][j];
      tempCoord4(j+1) = cartCoords[index4][j];
    }
    r12 << tempCoord2 - tempCoord1;
    r21 << tempCoord1 - tempCoord2;
    r23 << tempCoord3 - tempCoord2;
    r32 << tempCoord2 - tempCoord3;
    r34 << tempCoord4 - tempCoord3;
    r43 << tempCoord3 - tempCoord4;
    norm12 = r12.NormFrobenius();
    norm21 = r21.NormFrobenius();
    norm23 = r23.NormFrobenius();
    norm32 = r32.NormFrobenius();
    norm34 = r34.NormFrobenius();
    norm43 = r43.NormFrobenius();
#ifdef DEBUG
    cout << "norm12: " << norm12 << "\n";
    cout << "norm21: " << norm21 << "\n";
    cout << "norm23: " << norm23 << "\n";
    cout << "norm32: " << norm32 << "\n";
    cout << "norm34: " << norm34 << "\n";
    cout << "norm43: " << norm43 << "\n";
#endif
    e12 << r12 / norm12;
    e21 << r21 / norm21;
    e23 << r23 / norm23;
    e32 << r32 / norm32;
    e34 << r34 / norm34;
    e43 << r43 / norm43;
    angle123 = acos(DotProduct(r21,r23) / (norm21 * norm23));   // Wilson's angle 2
    angle234 = acos(DotProduct(r32,r34) / (norm32 * norm34));   // Wilson's angle 3
    cosAngle123 = DotProduct(r21,r23) / (norm21 * norm23);
    cosAngle234 = DotProduct(r32,r34) / (norm32 * norm34);
    sinAngle123 = sqrt(1 - (cosAngle123 * cosAngle123));
    sinAngle234 = sqrt(1 - (cosAngle234 * cosAngle234));
#ifdef DEBUG
    cout << "angle123: " << angle123 << ", cos(angle123): " << cos(angle123) << ", sin(angle123): " << sin(angle123) << "\n";
    cout << "angle234: " << angle234 << ", cos(angle234): " << cos(angle234) << ", sin(angle234): " << sin(angle234) << "\n";
    cout << "cosAngle123: " << cosAngle123 << ", sinAngle123: " << sinAngle123 << "\n";
    cout << "cosAngle234: " << cosAngle234 << ", sinAngle234: " << sinAngle234 << "\n";
#endif
    cross1223(1) = (e12(2)*e23(3)) - (e12(3)*e23(2));
    cross1223(2) = (e12(3)*e23(1)) - (e12(1)*e23(3));
    cross1223(3) = (e12(1)*e23(2)) - (e12(2)*e23(1));
    cross4332(1) = (e43(2)*e32(3)) - (e43(3)*e32(2));
    cross4332(2) = (e43(3)*e32(1)) - (e43(1)*e32(3));
    cross4332(3) = (e43(1)*e32(2)) - (e43(2)*e32(1));
#ifdef DEBUG
    cout << "cross1223 (norm " << cross1223.NormFrobenius() << "):\n";
    cout << setw(9) << setprecision(6) << (cross1223);
    cout << "\n\n";
    cout << "cross4332 (norm " << cross4332.NormFrobenius() << "):\n";
    cout << setw(9) << setprecision(6) << (cross4332);
    cout << "\n\n";
#endif
    // First elements of coordinate triples
    tempCoord5 << -((cross1223 * scaleFactor) / (norm12 * sinAngle123 * sinAngle123));
    for (j=1; j<=3; j++) {
      B(i+numBonds+numAngles+1,((index1)*3)+j) = tempCoord5(j);
    }
    // Second elements of coordinate triples
    tempCoord5 << ((norm23 - (norm12 * cosAngle123)) / (norm23 * norm12 * sinAngle123 * sinAngle123)) * (cross1223);
    tempCoord6 << (cosAngle234 / (norm23 * sinAngle234 * sinAngle234)) * (cross4332);
#ifdef DEBUG
    cout << "tempCoord5:\n";
    cout << setw(9) << setprecision(6) << (tempCoord5);
    cout << "tempCoord6:\n";
    cout << setw(9) << setprecision(6) << (tempCoord6);
#endif
    tempCoord5 << (tempCoord5 + tempCoord6) * scaleFactor;
#ifdef DEBUG
    cout << "tempCoord5:\n";
    cout << setw(9) << setprecision(6) << (tempCoord5);
#endif
    for (j=1; j<=3; j++) {
      B(i+numBonds+numAngles+1,((index2)*3)+j) = tempCoord5(j);
    }
    // Third elements of coordinate triples
    tempCoord5 << ((norm32 - (norm43 * cosAngle234)) / (norm32 * norm43 * sinAngle234 * sinAngle234)) * (cross4332);
    tempCoord6 << (cosAngle123 / (norm32 * sinAngle123 * sinAngle123)) * (cross1223);
    tempCoord5 << (tempCoord5 + tempCoord6) * scaleFactor;
    for (j=1; j<=3; j++) {
      B(i+numBonds+numAngles+1,((index3)*3)+j) = tempCoord5(j);
    }
    // Fourth elements of coordinate triples
    tempCoord5 << -((cross4332 * scaleFactor) / (norm43 * sinAngle234 * sinAngle234));
    for (j=1; j<=3; j++) {
      B(i+numBonds+numAngles+1,((index4)*3)+j) = tempCoord5(j);
    }
  }
  
#ifdef DEBUG
  cout << "B:\n";
  cout << setw(9) << setprecision(3) << (B);
  cout << "\n\n";
#endif

  // Impropers
  RowVector r41(3);   // Vector from 4th to 1st point
  RowVector r42(3);   // Vector from 4th to 2nd point
  RowVector e41(3);   // Unit vector from 4th to 1st point
  RowVector e42(3);   // Unit vector from 4th to 2nd point
  RowVector normVector(3);   // Normal to the plane
  Real norm41;        // Norm of r41
  Real norm42;        // Norm of r42
  Real angle142 = 0.0;   // Angle in radians
  Real angle143 = 0.0;   // Angle in radians
  Real angle243 = 0.0;   // Angle in radians
  Real cosAngle142 = 0.0;
  Real cosAngle143 = 0.0;
  Real cosAngle243 = 0.0;
  Real sinAngle142 = 0.0;
  Real sinAngle143 = 0.0;
  Real sinAngle243 = 0.0;
  Real apexCoeff = 0.0;   // Magnitude of central atom displacement
  scaleFactor = -0.352313;   // Scale factor (-0.352313)
  for (i=0; i<numImpropers; i++) {
    index1 = impropers[i]->x1;
    index2 = impropers[i]->x2;
    index3 = impropers[i]->x3;
    index4 = impropers[i]->x4;
    for (j=0; j<3; j++) {
      tempCoord1(j+1) = cartCoords[index1][j];
      tempCoord2(j+1) = cartCoords[index2][j];
      tempCoord3(j+1) = cartCoords[index3][j];
      tempCoord4(j+1) = cartCoords[index4][j];
    }
    r41 << tempCoord1 - tempCoord4;
    r42 << tempCoord2 - tempCoord4;
    r43 << tempCoord3 - tempCoord4;
    norm41 = r41.NormFrobenius();
    norm42 = r42.NormFrobenius();
    norm43 = r43.NormFrobenius();
    e41 << r41 / norm41;
    e42 << r42 / norm42;
    e43 << r43 / norm43;
    angle142 = acos(DotProduct(r41,r42) / (norm41 * norm42));
    angle143 = acos(DotProduct(r41,r43) / (norm41 * norm43));
    angle243 = acos(DotProduct(r42,r43) / (norm42 * norm43));
    cosAngle142 = DotProduct(r41,r42) / (norm41 * norm42);
    cosAngle143 = DotProduct(r41,r43) / (norm41 * norm43);
    cosAngle243 = DotProduct(r42,r43) / (norm42 * norm43);
    sinAngle142 = sqrt(1 - (cosAngle142 * cosAngle142));
    sinAngle143 = sqrt(1 - (cosAngle143 * cosAngle143));
    sinAngle243 = sqrt(1 - (cosAngle243 * cosAngle243));
    normVector(1) = (r41(2)*r42(3)) - (r41(3)*r42(2));
    normVector(2) = (r41(3)*r42(1)) - (r41(1)*r42(3));
    normVector(3) = (r41(1)*r42(2)) - (r41(2)*r42(1));
    normVector << normVector / normVector.NormFrobenius();
    // First elements of coordinate triples
    tempCoord5 << normVector * (scaleFactor / norm41);
    for (j=1; j<=3; j++) {
      B(i+numBonds+numAngles+numDihedrals+1,((index1)*3)+j) = tempCoord5(j);
    }
    // Second elements of coordinate triples
    tempCoord5 << normVector * sinAngle143 * scaleFactor;
    tempCoord5 << tempCoord5 / (norm42 * sinAngle243);
    for (j=1; j<=3; j++) {
      B(i+numBonds+numAngles+numDihedrals+1,((index2)*3)+j) = tempCoord5(j);
    }
    // Third elements of coordinate triples
    tempCoord5 << normVector * sinAngle142 * scaleFactor;
    tempCoord5 << tempCoord5 / (norm43 * sinAngle243);
    for (j=1; j<=3; j++) {
      B(i+numBonds+numAngles+numDihedrals+1,((index3)*3)+j) = tempCoord5(j);
    }
    // Fourth elements of coordinate triples
    apexCoeff = -1.0 / norm42;
    apexCoeff -= sinAngle143 / (norm42 * sinAngle243);
    apexCoeff -= sinAngle142 / (norm43 * sinAngle243);
    tempCoord5 << normVector * apexCoeff * scaleFactor;
    for (j=1; j<=3; j++) {
      B(i+numBonds+numAngles+numDihedrals+1,((index4)*3)+j) = tempCoord5(j);
    }
  }
  
  return;
}
Exemplo n.º 28
0
//--------------------------------------------------------------------------------------------------
void q3Body::CalculateMassData( )
{
	q3Mat3 inertia = q3Diagonal( r32( 0.0 ) );
	m_invInertiaModel = q3Diagonal( r32( 0.0 ) );
	m_invInertiaWorld = q3Diagonal( r32( 0.0 ) );
	m_invMass = r32( 0.0 );
	m_mass = r32( 0.0 );
	r32 mass = r32( 0.0 );

	if ( m_flags & eStatic || m_flags &eKinematic )
	{
		q3Identity( m_localCenter );
		m_worldCenter = m_tx.position;
		return;
	}

	q3Vec3 lc;
	q3Identity( lc );

	for ( q3Box* box = m_boxes; box; box = box->next)
	{
		if ( box->density == r32( 0.0 ) )
			continue;

		q3MassData md;
		box->ComputeMass( &md );
		mass += md.mass;
		inertia += md.inertia;
		lc += md.center * md.mass;
	}

	if ( mass > r32( 0.0 ) )
	{
		m_mass = mass;
		m_invMass = r32( 1.0 ) / mass;
		lc *= m_invMass;
		q3Mat3 identity;
		q3Identity( identity );
		inertia -= (identity * q3Dot( lc, lc ) - q3OuterProduct( lc, lc )) * mass;
		m_invInertiaModel = q3Inverse( inertia );

		if ( m_flags & eLockAxisX )
			q3Identity( m_invInertiaModel.ex );

		if ( m_flags & eLockAxisY )
			q3Identity( m_invInertiaModel.ey );

		if ( m_flags & eLockAxisZ )
			q3Identity( m_invInertiaModel.ez );
	}
	else
	{
		// Force all dynamic bodies to have some mass
		m_invMass = r32( 1.0 );
		m_invInertiaModel = q3Diagonal( r32( 0.0 ) );
		m_invInertiaWorld = q3Diagonal( r32( 0.0 ) );
	}

	m_localCenter = lc;
	m_worldCenter = q3Mul( m_tx, lc );
}
Exemplo n.º 29
0
void tms34010_disassembler::print_long_parm(std::ostream &stream, offs_t &pos, const data_buffer &params)
{
	util::stream_format(stream, "%Xh", r32(pos, params));
}
Exemplo n.º 30
0
	const OperandREG16 RegisterAllocator::r16(const OperandREF &ref, bool copy)
	{
		return (OperandREG16)r32(ref, copy, 2);
	}