/*
* 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),
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);
}
}
}
//--------------------------------------------------------------------------------------------------
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;
}
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]);
}
//--------------------------------------------------------------------------------------------------
void q3Body::SetToAwake( )
{
if( !(m_flags & eAwake) )
{
m_flags |= eAwake;
m_sleepTime = r32( 0.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;
}
unint line32 (addr address)
{
unint i;
for (i = 0; i < U32_LINE; i++, address += sizeof(u32)) {
printf(Format32[Zerofill], r32(address));
}
return U8_LINE;
}
//--------------------------------------------------------------------------------------------------
void q3Body::SetToSleep( )
{
m_flags &= ~eAwake;
m_sleepTime = r32( 0.0 );
q3Identity( m_linearVelocity );
q3Identity( m_angularVelocity );
q3Identity( m_force );
q3Identity( m_torque );
}
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);
}
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));
}
}
}
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();
}
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);
}
//--------------------------------------------------------------------------------------------------
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;
}
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);
}
//--------------------------------------------------------------------------------------------------
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;
}
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);
}
}
}
}
//--------------------------------------------------------------------------------------------------
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;
}
}
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
}
quint64 TagDataStream::r64() {
quint64 r = (quint64)r32() << 32;
r |= r32();
return r;
}
const SoftWire::OperandREG32 r()
{
return r32(&x[q()]);
}
quint64 Handle::r64() {
quint64 r = r32();
r |= ((quint64)r32()) << 32;
return r;
}
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');
}
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();
}
}
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);
}
/**
* 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;
}
//--------------------------------------------------------------------------------------------------
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 );
}
void tms34010_disassembler::print_long_parm(std::ostream &stream, offs_t &pos, const data_buffer ¶ms)
{
util::stream_format(stream, "%Xh", r32(pos, params));
}
const OperandREG16 RegisterAllocator::r16(const OperandREF &ref, bool copy)
{
return (OperandREG16)r32(ref, copy, 2);
}