/*
* Idle Function - moves eyeposition
*/
void agvMove(void)
{
switch (MoveMode) {
case FLYING:
Ex += EyeMove*sin(TORAD(EyeAz))*cos(TORAD(EyeEl));
Ey += EyeMove*sin(TORAD(EyeEl));
Ez -= EyeMove*cos(TORAD(EyeAz))*cos(TORAD(EyeEl));
break;
case POLAR:
EyeEl += ElSpin;
EyeAz += AzSpin;
if (ConstrainEl()) { /* weird spin thing to make things look */
ElSpin = -ElSpin; /* look better when you are kept from going */
/* upside down while spinning - Isn't great */
if (fabs(ElSpin) > fabs(AzSpin))
AzSpin = fabs(ElSpin) * ((AzSpin > 0) ? 1 : -1);
}
break;
}
if (AdjustingAzEl) {
dAz *= SLOW_DAZ;
dEl *= SLOW_DEL;
}
if (AllowIdle) {
glutSetWindow(RedisplayWindow);
glutPostRedisplay();
}
}
static void iterate( void ) {
ALfloat orientation[] = { 0.0f, 0.0f, -1.0f,
0.0f, 1.0f, 0.0f };
ALfloat xaxis[] = { 1.0f, 0.0f, 0.0f };
ALfloat yaxis[] = { 0.0f, 1.0f, 0.0f };
ALfloat zaxis[] = { 0.0f, 0.0f, 1.0f };
ALfloat *at = orientation;
ALfloat *up = &orientation[3];
static ALint angle = 0;
/*
* rotate up vector about at vector by angle degrees.
*/
_alRotatePointAboutAxis(TORAD(angle), up, at);
fprintf(stderr, "orientation: \n\tAT(%f %f %f)\n\tUP(%f %f %f)\n",
orientation[0], orientation[1], orientation[2],
orientation[3], orientation[4], orientation[5]);
fprintf(stderr, "angle = %d\n", angle);
angle += 15; /* increment fifeteen degrees degree */
alListenerfv(AL_ORIENTATION, orientation);
micro_sleep(900000);
}
/*
* I took the idea of tracking eye position in absolute
* coords and direction looking in Polar form from denis
*/
void FlyLookFrom(GLfloat x, GLfloat y, GLfloat z, GLfloat az, GLfloat el)
{
float lookat[3], perp[3], up[3];
lookat[0] = sin(TORAD(az))*cos(TORAD(el));
lookat[1] = sin(TORAD(el));
lookat[2] = -cos(TORAD(az))*cos(TORAD(el));
normalize(lookat);
perp[0] = lookat[2];
perp[1] = 0;
perp[2] = -lookat[0];
normalize(perp);
ncrossprod(lookat, perp, up);
gluLookAt(x, y, z,
x+lookat[0], y+lookat[1], z+lookat[2],
up[0], up[1], up[2]);
}
// adds all the shapes to a vector, that is used in the Update function.
void buildScene()
{
if (shapes.size() == 0)
{
// a plane facing the camera, passing through point (0,0,1000)
shapes.push_back(dynamic_cast<Shape*>(new Plane(Vec(0, 0, -1), -1000, Color(0, 30, 30))));
// a sphere at (400,400,300), with radius 200
shapes.push_back(dynamic_cast<Shape*>(new Sphere(Vec(400, 400, 300), 200, Color(100, 100, 0))));
// two triangles
shapes.push_back(dynamic_cast<Shape*>(new Triangle({ 350, 100, 200 }, { 300,100,200 }, { 400,700, 30 }, { 200,0,0 })));
shapes.push_back(dynamic_cast<Shape*>(new Triangle({ 100,300,0 }, { 150,300,0 }, { 100,100,0 }, { 0,0,255 })));
shapes.push_back(dynamic_cast<Shape*>(new Triangle({ 150,300,0 }, { 150,100,0 }, { 100,100,0 }, { 0,255,255 })));
float size = 20;
Vec p0{ -1, 0, 0 }; Vec p1{ 1, 0, 0 }; Vec p2{ 0,2*size,0 };
for (unsigned int i = 1; i <= 20; i++)
{
Vec off { size*2 * i, 700.0, 0.0 };
p0.x = cosf(TORAD(i * (90/20.0f))) * size; p0.z = -sinf(TORAD(i * (90/20.0f))) * size;
p1.x = -p0.x; p1.z = -p0.z;
shapes.push_back(dynamic_cast<Shape*>(new Triangle(p0 + off, p1 + off, p2 + off, { 255,255,0 })));
}
// one OBB touching the sphere on the side
// base
Vec b1{ 1, 0, 0 };
Vec b2{ 0, 1, 0 };
Vec b3{ 0, 0, 1 };
float angle{0.5};
// rotate around Z the basis
b1.x = cosf(angle); b1.y = -sinf(angle);
b2.x = sinf(angle); b2.y = cosf(angle);
shapes.push_back(dynamic_cast<Shape*>(new OBB(Vec(g_ToScreen->mScreenWidth/2, g_ToScreen->mScreenHeight/2, 100), b1, b2, b3, 50, 50, 50, { 0,255,0 })));
// further rotate around X the basis
float tempY = b1.y * cosf(angle) + b1.z * sinf(angle);
b1.z = b1.y * -sinf(angle) + b1.z * cosf(angle);
b1.y = tempY;
tempY = b2.y * cosf(angle) + b2.z * sinf(angle);
b2.z = b2.y * -sinf(angle) + b2.z * cosf(angle);
b2.y = tempY;
shapes.push_back(dynamic_cast<Shape*>(new OBB(Vec(200,600,400), b1,b2,b3, 100, 100, 100, {255,0,0})));
}
}
static void math_tan (void)
{
double d;
lua_Object o = lua_getparam (1);
if (o == NULL)
{ lua_error ("too few arguments to function `tan'"); return; }
if (!lua_isnumber(o))
{ lua_error ("incorrect arguments to function `tan'"); return; }
d = lua_getnumber(o);
lua_pushnumber (tan(TORAD(d)));
}
/*
* change EyeEl and EyeAz and position when mouse is moved w/ button down
*/
void agvHandleMotion(int x, int y)
{
int deltax = x - downx, deltay = y - downy;
switch (downb) {
case GLUT_LEFT_BUTTON:
EyeEl = downEl + EL_SENS * ((MoveMode == FLYING) ? -deltay : deltay);
ConstrainEl();
EyeAz = downAz + AZ_SENS * deltax;
dAz = PREV_DAZ*dAz + CUR_DAZ*(lastAz - EyeAz);
dEl = PREV_DEL*dEl + CUR_DEL*(lastEl - EyeEl);
lastAz = EyeAz;
lastEl = EyeEl;
break;
case GLUT_MIDDLE_BUTTON:
EyeDist = downDist + DIST_SENS*deltay;
Ex = downEx - E_SENS*deltay*sin(TORAD(EyeAz))*cos(TORAD(EyeEl));
Ey = downEy - E_SENS*deltay*sin(TORAD(EyeEl));
Ez = downEz + E_SENS*deltay*cos(TORAD(EyeAz))*cos(TORAD(EyeEl));
break;
}
glutPostRedisplay();
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray*prhs[])
{
HMatrix R;
EulerAngles sa, ra;
const double *p_inangles;
mxArray *outangles;
const mwSize dims[]={3,1};
/*get angles*/
p_inangles=mxGetData(prhs[0]);
sa.x=(float)p_inangles[0];
sa.y=(float)p_inangles[1];
sa.z=(float)p_inangles[2];
/*convert angles from degrees to radians*/
TORAD(sa.x);
TORAD(sa.y);
TORAD(sa.z);
sa.w = EulOrdZYZr;
Eul_ToHMatrix(sa, R);
ra = Eul_FromHMatrix(R, EulOrdZXZr);
/*convert angles from radians to degrees*/
TODEG(ra.x);
TODEG(ra.y);
TODEG(ra.z);
if ((plhs[0] = mxCreateNumericArray(2,dims,mxSINGLE_CLASS,mxREAL))==NULL) { mexErrMsgTxt("Memory allocation problem in tom_eulerconvert.\n"); }
outangles = mxGetData(plhs[0]);
((float*)outangles)[0] = ra.x;
((float*)outangles)[1] = ra.y;
((float*)outangles)[2] = ra.z;
/*printf("rotating angles = %10.3f %10.3f %10.3f\n", ra.x, ra.y, ra.z);*/
}
/*
* when moving to flying we stay in the same spot, moving to polar we
* reset since we have to be looking at the origin (though a pivot from
* current position to look at origin might be cooler)
*/
void agvSwitchMoveMode(int move)
{
switch (move) {
case FLYING:
if (MoveMode == FLYING) return;
Ex = -EyeDist*sin(TORAD(EyeAz))*cos(TORAD(EyeEl));
Ey = EyeDist*sin(TORAD(EyeEl));
Ez = EyeDist*(cos(TORAD(EyeAz))*cos(TORAD(EyeEl)));
EyeAz = EyeAz;
EyeEl = -EyeEl;
EyeMove = INIT_MOVE;
break;
case POLAR:
EyeDist = INIT_DIST;
EyeAz = INIT_POLAR_AZ;
EyeEl = INIT_POLAR_EL;
AzSpin = INIT_AZ_SPIN;
ElSpin = INIT_EL_SPIN;
break;
}
MoveMode = move;
MoveOn(1);
glutPostRedisplay();
}
static void iterate( void ) {
ALfloat orientation[] = { 0.0f, 0.0f, -1.0f,
0.0f, 1.0f, 0.0f };
static ALint angle = 0;
/*
* rotate at vector about up vector by angle degrees.
*/
_alRotatePointAboutAxis(TORAD(angle), orientation, &orientation[3]);
angle += 15; /* increment fifeteen degrees degree */
fprintf(stderr, "orientation: \n\tAT(%f %f %f)\n\tUP(%f %f %f)\n",
orientation[0], orientation[1], orientation[2],
orientation[3], orientation[4], orientation[5]);
alListenerfv(AL_ORIENTATION, orientation);
micro_sleep(1500000);
}
static void motion (int x, int y)
{
if (!VManipulator::getCurrent())
return;
int dx = abs(x-g_x0);
int dy = abs(y-g_y0);
if ( dx > 1 || dy > 1) {
if (g_state == 'r') {
VVector v0 = map(g_x0, g_y0);
VVector v1 = map(x, y);
VVector r = Cross(v0, v1);
VManipulator::getCurrent()->rotate(TORAD(2*asin(r.Length())),r.x,r.y,r.z);
}
else if (g_state == 's') {
int vp[4];
glGetIntegerv(GL_VIEWPORT,vp);
float f = dx > dy ? (float)(x-g_x0) / vp[2] : (float) (-y+g_y0) / vp[3];
VManipulator::getCurrent()->scale(1+f, 1+f, 1+f);
}
g_x0 = x; g_y0 = y;
glutPostRedisplay();
}
}
static int math_cos (lua_State *L) {
lua_pushnumber(L, cos(TORAD(luaL_checknumber(L, 1))));
return 1;
}
static int math_tan (lua_State *L) {
lua_pushnumber(L, tan(TORAD(luaL_check_number(L, 1))));
return 1;
}
static void math_cos() {
lua_pushnumber(cos(TORAD(luaL_check_number(1))));
}
static void math_tan() {
lua_pushnumber(tan(TORAD(luaL_check_number(1))));
}
static void math_tan (void)
{
double d = lua_check_number(1, "tan");
lua_pushnumber (tan(TORAD(d)));
}
void Character::Control(GLdouble FrameInterval)
{
GLdouble step = WALK_SPEED;
if(bSpecial[GLUT_KEY_SHIFT_L])
step *= SPRINT_KOEF;
if(bKeyboard['W']) {
dVelocityX -= FrameInterval*AIR_ACCEL*step*sin(TORAD(dSpinY));
dVelocityZ -= FrameInterval*AIR_ACCEL*step*cos(TORAD(dSpinY));
}
if(bKeyboard['S']) {
dVelocityX += FrameInterval*AIR_ACCEL*step*sin(TORAD(dSpinY));
dVelocityZ += FrameInterval*AIR_ACCEL*step*cos(TORAD(dSpinY));
}
if(bKeyboard['D']) {
dVelocityX += FrameInterval*AIR_ACCEL*step*cos(TORAD(dSpinY));
dVelocityZ -= FrameInterval*AIR_ACCEL*step*sin(TORAD(dSpinY));
}
if(bKeyboard['A']) {
dVelocityX -= FrameInterval*AIR_ACCEL*step*cos(TORAD(dSpinY));
dVelocityZ += FrameInterval*AIR_ACCEL*step*sin(TORAD(dSpinY));
}
if(bKeyboard['R']) {
dVelocityY += FrameInterval*AIR_ACCEL*step;
}
if(bKeyboard['F']) {
dVelocityY -= FrameInterval*AIR_ACCEL*step;
}
GLdouble ko = dVelocityX*dVelocityX + dVelocityZ*dVelocityZ;
if(ko > WALK_SPEED*WALK_SPEED*SPRINT_KOEF*SPRINT_KOEF) {
ko = pow(ko, 0.5);
dVelocityX = dVelocityX*WALK_SPEED*SPRINT_KOEF/ko;
dVelocityZ = dVelocityZ*WALK_SPEED*SPRINT_KOEF/ko;
}
if(bKeyboard[VK_SPACE]) {
}
if(bKeyboard['X']) {
dVelocityX = 0;
dVelocityY = 0;
dVelocityZ = 0;
}
GLdouble yerr, xerr, zerr;
GetPlane(&xerr, &yerr, &zerr);
PosInWorld pos;
if(zerr < xerr && zerr < yerr) {
if((position.bz < centerPos.bz && position.cz == centerPos.cz) || position.cz < centerPos.cz) {
pos = PosInWorld(0, 0, 0.5);
} else {
pos = PosInWorld(0, 0, -0.5);
}
} else if(xerr < zerr && xerr < yerr) {
if((position.bx < centerPos.bx && position.cx == centerPos.cx) || position.cx < centerPos.cx) {
pos = PosInWorld(0.5, 0, 0);
} else {
pos = PosInWorld(-0.5, 0, 0);
}
} else if(yerr < xerr && yerr < zerr) {
if(position.by < centerPos.by) {
pos = PosInWorld(0, 0.5, 0);
} else {
pos = PosInWorld(0, -0.5, 0);
}
}
aimedBlock = BlockInWorld(centerPos + pos);
freeBlock = BlockInWorld(centerPos + pos.inv());
int num = 100;
int sq = 100;
int sqb2 = sq/2;
if(bKeyboard['1']) {
int i = 0;
while(i < num) {
if(wWorld.AddBlock(BlockInWorld(rand()%sq-sqb2, abs(rand()%sq-sqb2), rand()%sq-sqb2), rand()%14+1, true))
i++;
}
}
if(bKeyboard['2']) {
int i = 0;
while(i < num) {
if(wWorld.RemoveBlock(BlockInWorld(rand()%sq-sqb2, rand()%sq-sqb2, rand()%sq-sqb2), true))
i++;
}
}
if(bKeyboard['3']) {
for(BlockCoord i = -sqb2; i <= sqb2; i++) {
for(BlockCoord j = -sqb2; j <= sqb2; j++) {
for(BlockCoord k = -sqb2; k <= sqb2; k++) {
wWorld.RemoveBlock(BlockInWorld(i, j, k), true);
}
}
}
}
if(bKeyboard['4']) {
wWorld.LoadChunk(0, 0);
bKeyboard['4'] = false;
}
if(bKeyboard['5']) {
wWorld.UnLoadChunk(0, 0);
bKeyboard['5'] = false;
}
if(bKeyboard['6']) {
for(int i = 0; i < 8; i++)
for(int j = 0; j < 8; j++)
wWorld.LoadChunk(i, j);
bKeyboard['6'] = false;
}
if(bKeyboard['7']) {
for(int i = 0; i < 8; i++)
for(int j = 0; j < 8; j++)
wWorld.UnLoadChunk(i, j);
bKeyboard['7'] = false;
}
if(bKeyboard['0']) {
static Param par = {0, 1, &wWorld};
_beginthread(LoadNGenerate, 0, &par);
WaitForSingleObject(wWorld.parget2, INFINITE);
ResetEvent(wWorld.parget2);
par.z++;
bKeyboard['0'] = false;
}
if(bKeyboard['C']) {
Chunk *chunk;
int index;
wWorld.FindBlock(aimedBlock, &chunk, &index);
if ((chunk)&&(chunk->bBlocks[index].cMaterial == MAT_DIRT)) {
chunk->bBlocks[index].bVisible ^= (1 << SNOWCOVERED);
}
}
if(bKeyboard['V']) {
Chunk *chunk;
int index;
wWorld.FindBlock(aimedBlock, &chunk, &index);
if ((chunk)&&(chunk->bBlocks[index].cMaterial == MAT_DIRT)) {
chunk->bBlocks[index].bVisible ^= (1 << GRASSCOVERED);
}
}
if(bKeyboard['E']) {
wWorld.RemoveBlock(aimedBlock, true);
}
if(bKeyboard['Q']) {
wWorld.AddBlock(freeBlock, MAT_PUMPKIN_SHINE, true);
}
if(bKeyboard['O']) {
wWorld.SaveChunks();
}
position = position + PosInWorld(FrameInterval*dVelocityX, FrameInterval*dVelocityY, FrameInterval*dVelocityZ);
/*{
signed short xx, yy, zz;
GLdouble wx = gfPosX + gfVelX;
GLdouble wy = gfPosY - PLAYER_HEIGHT + 0.1;
GLdouble wz = gfPosZ;
xx = floor(wx/TILE_SIZE + 0.5);
zz = floor(wz/TILE_SIZE + 0.5);
yy = floor(wy/TILE_SIZE);
if((FindTile(xx, yy, zz) == NULL)&&(FindTile(xx, yy + 1, zz) == NULL))
gfPosX += g_FrameInterval*gfVelX;
else gfVelX = 0;
}
{
signed short xx, yy, zz;
GLdouble wx = gfPosX;
GLdouble wy = gfPosY - PLAYER_HEIGHT + 0.1;
GLdouble wz = gfPosZ + gfVelZ;
xx = floor(wx/TILE_SIZE + 0.5);
zz = floor(wz/TILE_SIZE + 0.5);
yy = floor(wy/TILE_SIZE);
if((FindTile(xx, yy, zz) == NULL)&&(FindTile(xx, yy + 1, zz) == NULL))
gfPosZ += g_FrameInterval*gfVelZ;
else gfVelZ = 0;
}
*/
/*if(falling)
{
gfPosY -= g_FrameInterval*gfVelY;
if(gfVelY < MAX_DOWNSTEP)
gfVelY += g_FrameInterval*STEP_DOWNSTEP;
}*/
/*
{
signed short xx, yy, zz;
GLdouble wx = gfPosX;
GLdouble wy = gfPosY - PLAYER_HEIGHT;
GLdouble wz = gfPosZ;
xx = floor(wx/TILE_SIZE + 0.5);
zz = floor(wz/TILE_SIZE + 0.5);
yy = floor(wy/TILE_SIZE);
if(FindTile(xx, yy, zz) == NULL) falling = true;
else
{
gfVelY = 0;
if(falling)
{
falling = false;
gfPosY = (yy + 1)*TILE_SIZE + PLAYER_HEIGHT - 0.001;
}
}
}
if(!falling)
{
gfVelX = 0;
gfVelZ = 0;
}*/
//falling = 1;
// if(dPositionX >= LOCATION_SIZE_XZ*TILE_SIZE) { dPositionX -= LOCATION_SIZE_XZ*TILE_SIZE; lnwPositionX++;}
// if(dPositionX < 0) { dPositionX += LOCATION_SIZE_XZ*TILE_SIZE; lnwPositionX--;}
// if(dPositionZ >= LOCATION_SIZE_XZ*TILE_SIZE) { dPositionZ -= LOCATION_SIZE_XZ*TILE_SIZE; lnwPositionZ++;}
// if(dPositionZ < 0) { dPositionZ += LOCATION_SIZE_XZ*TILE_SIZE; lnwPositionZ--;}
}
static void math_sin (void)
{
double d = lua_check_number(1, "sin");
lua_pushnumber (sin(TORAD(d)));
}
static void math_cos (void)
{
double d = lua_check_number(1, "cos");
lua_pushnumber (cos(TORAD(d)));
}
void icosaeder_vertices(real *xus)
{
rh = sqrt(1.-2.*cos(TORAD(72.)))/(1.-cos(TORAD(72.)));
rg = cos(TORAD(72.))/(1.-cos(TORAD(72.)));
/* icosaeder vertices */
xus[ 0] = 0.; xus[ 1] = 0.; xus[ 2] = 1.;
xus[ 3] = rh*cos(TORAD(72.)); xus[ 4] = rh*sin(TORAD(72.)); xus[ 5] = rg;
xus[ 6] = rh*cos(TORAD(144.)); xus[ 7] = rh*sin(TORAD(144.)); xus[ 8] = rg;
xus[ 9] = rh*cos(TORAD(216.)); xus[10] = rh*sin(TORAD(216.)); xus[11] = rg;
xus[12] = rh*cos(TORAD(288.)); xus[13] = rh*sin(TORAD(288.)); xus[14] = rg;
xus[15] = rh; xus[16] = 0; xus[17] = rg;
xus[18] = rh*cos(TORAD(36.)); xus[19] = rh*sin(TORAD(36.)); xus[20] = -rg;
xus[21] = rh*cos(TORAD(108.)); xus[22] = rh*sin(TORAD(108.)); xus[23] = -rg;
xus[24] = -rh; xus[25] = 0; xus[26] = -rg;
xus[27] = rh*cos(TORAD(252.)); xus[28] = rh*sin(TORAD(252.)); xus[29] = -rg;
xus[30] = rh*cos(TORAD(324.)); xus[31] = rh*sin(TORAD(324.)); xus[32] = -rg;
xus[33] = 0.; xus[34] = 0.; xus[35] = -1.;
}
int ico_dot_arc(int densit) /* densit...required dots per unit sphere */
{
/* dot distribution on a unit sphere based on an icosaeder *
* great circle average refining of icosahedral face */
int i, j, k, tl, tl2, tn, tess;
real a, d, x, y, z, x2, y2, z2, x3, y3, z3;
real xij, yij, zij, xji, yji, zji, xik, yik, zik, xki, yki, zki,
xjk, yjk, zjk, xkj, ykj, zkj;
real *xus = NULL;
/* calculate tessalation level */
a = sqrt((((real) densit)-2.)/10.);
tess = (int) ceil(a);
n_dot = 10*tess*tess+2;
if (n_dot < densit)
{
ERROR("ico_dot_arc: error in formula for tessalation level (%d->%d, %d)",
tess, n_dot, densit);
}
snew(xus, 3*n_dot);
xpunsp = xus;
icosaeder_vertices(xus);
if (tess > 1)
{
tn = 12;
a = rh*rh*2.*(1.-cos(TORAD(72.)));
/* calculate tessalation of icosaeder edges */
for (i = 0; i < 11; i++)
{
for (j = i+1; j < 12; j++)
{
x = xus[3*i]-xus[3*j];
y = xus[1+3*i]-xus[1+3*j]; z = xus[2+3*i]-xus[2+3*j];
d = x*x+y*y+z*z;
if (fabs(a-d) > DP_TOL)
{
continue;
}
for (tl = 1; tl < tess; tl++)
{
if (tn >= n_dot)
{
ERROR("ico_dot: tn exceeds dimension of xus");
}
divarc(xus[3*i], xus[1+3*i], xus[2+3*i],
xus[3*j], xus[1+3*j], xus[2+3*j],
tl, tess, &xus[3*tn], &xus[1+3*tn], &xus[2+3*tn]);
tn++;
}
}
}
/* calculate tessalation of icosaeder faces */
for (i = 0; i < 10; i++)
{
for (j = i+1; j < 11; j++)
{
x = xus[3*i]-xus[3*j];
y = xus[1+3*i]-xus[1+3*j]; z = xus[2+3*i]-xus[2+3*j];
d = x*x+y*y+z*z;
if (fabs(a-d) > DP_TOL)
{
continue;
}
for (k = j+1; k < 12; k++)
{
x = xus[3*i]-xus[3*k];
y = xus[1+3*i]-xus[1+3*k]; z = xus[2+3*i]-xus[2+3*k];
d = x*x+y*y+z*z;
if (fabs(a-d) > DP_TOL)
{
continue;
}
x = xus[3*j]-xus[3*k];
y = xus[1+3*j]-xus[1+3*k]; z = xus[2+3*j]-xus[2+3*k];
d = x*x+y*y+z*z;
if (fabs(a-d) > DP_TOL)
{
continue;
}
for (tl = 1; tl < tess-1; tl++)
{
divarc(xus[3*j], xus[1+3*j], xus[2+3*j],
xus[3*i], xus[1+3*i], xus[2+3*i],
tl, tess, &xji, &yji, &zji);
divarc(xus[3*k], xus[1+3*k], xus[2+3*k],
xus[3*i], xus[1+3*i], xus[2+3*i],
tl, tess, &xki, &yki, &zki);
for (tl2 = 1; tl2 < tess-tl; tl2++)
{
divarc(xus[3*i], xus[1+3*i], xus[2+3*i],
xus[3*j], xus[1+3*j], xus[2+3*j],
tl2, tess, &xij, &yij, &zij);
divarc(xus[3*k], xus[1+3*k], xus[2+3*k],
xus[3*j], xus[1+3*j], xus[2+3*j],
tl2, tess, &xkj, &ykj, &zkj);
divarc(xus[3*i], xus[1+3*i], xus[2+3*i],
xus[3*k], xus[1+3*k], xus[2+3*k],
tess-tl-tl2, tess, &xik, &yik, &zik);
divarc(xus[3*j], xus[1+3*j], xus[2+3*j],
xus[3*k], xus[1+3*k], xus[2+3*k],
tess-tl-tl2, tess, &xjk, &yjk, &zjk);
if (tn >= n_dot)
{
ERROR("ico_dot: tn exceeds dimension of xus");
}
divarc(xki, yki, zki, xji, yji, zji, tl2, tess-tl,
&x, &y, &z);
divarc(xkj, ykj, zkj, xij, yij, zij, tl, tess-tl2,
&x2, &y2, &z2);
divarc(xjk, yjk, zjk, xik, yik, zik, tl, tl+tl2,
&x3, &y3, &z3);
x = x+x2+x3; y = y+y2+y3; z = z+z2+z3;
d = sqrt(x*x+y*y+z*z);
xus[3*tn] = x/d;
xus[1+3*tn] = y/d;
xus[2+3*tn] = z/d;
tn++;
} /* cycle tl2 */
} /* cycle tl */
} /* cycle k */
} /* cycle j */
} /* cycle i */
if (n_dot != tn)
{
ERROR("ico_dot: n_dot(%d) and tn(%d) differ", n_dot, tn);
}
} /* end of if (tess > 1) */
return n_dot;
} /* end of routine ico_dot_arc */
static void math_sin (void)
{
lua_pushnumber(sin(TORAD(luaL_check_number(1))));
}
void HavokExport::makeHavokRigidBody(NiNodeRef parent, INode *ragdollParent, float scale) {
this->scale = scale;
Object *Obj = ragdollParent->GetObjectRef();
Modifier* rbMod = nullptr;
Modifier* shapeMod = nullptr;
Modifier* constraintMod = nullptr;
SimpleObject* havokTaperCapsule = nullptr;
//get modifiers
while (Obj->SuperClassID() == GEN_DERIVOB_CLASS_ID) {
IDerivedObject *DerObj = static_cast<IDerivedObject *> (Obj);
const int nMods = DerObj->NumModifiers(); //it is really the last modifier on the stack, and not the total number of modifiers
for (int i = 0; i < nMods; i++)
{
Modifier *Mod = DerObj->GetModifier(i);
if (Mod->ClassID() == HK_RIGIDBODY_MODIFIER_CLASS_ID) {
rbMod = Mod;
}
if (Mod->ClassID() == HK_SHAPE_MODIFIER_CLASS_ID) {
shapeMod = Mod;
}
if (Mod->ClassID() == HK_CONSTRAINT_RAGDOLL_CLASS_ID || Mod->ClassID() == HK_CONSTRAINT_HINGE_CLASS_ID) {
constraintMod = Mod;
}
}
if (Obj->SuperClassID() == GEOMOBJECT_CLASS_ID) {
havokTaperCapsule = (SimpleObject*)Obj;
}
Obj = DerObj->GetObjRef();
}
if (!rbMod) {
throw exception(FormatText("No havok rigid body modifier found on %s", ragdollParent->GetName()));
}
if (!shapeMod) {
throw exception(FormatText("No havok shape modifier found on %s", ragdollParent->GetName()));
}
// Object* taper = ragdollParent->GetObjectRef();
IParamBlock2* taperParameters = Obj->GetParamBlockByID(PB_TAPEREDCAPSULE_OBJ_PBLOCK);
float radius;
enum
{
// GENERAL PROPERTIES ROLLOUT
PA_TAPEREDCAPSULE_OBJ_RADIUS = 0,
PA_TAPEREDCAPSULE_OBJ_TAPER,
PA_TAPEREDCAPSULE_OBJ_HEIGHT,
PA_TAPEREDCAPSULE_OBJ_VERSION_INTERNAL,
};
taperParameters->GetValue(PA_TAPEREDCAPSULE_OBJ_RADIUS, 0, radius, FOREVER);
int shapeType;
if (IParamBlock2* shapeParameters = shapeMod->GetParamBlockByID(PB_SHAPE_MOD_PBLOCK)) {
shapeParameters->GetValue(PA_SHAPE_MOD_SHAPE_TYPE,0,shapeType,FOREVER);
}
//Havok Shape
bhkShapeRef shape;
if (shapeType == 2) {
// Capsule
bhkCapsuleShapeRef capsule = new bhkCapsuleShape();
capsule->SetRadius(radius/scale);
capsule->SetRadius1(radius/scale);
capsule->SetRadius2(radius/scale);
float length;
taperParameters->GetValue(PA_TAPEREDCAPSULE_OBJ_HEIGHT, 0, length, FOREVER);
//get the normal
Matrix3 axis(true);
ragdollParent->GetObjOffsetRot().MakeMatrix(axis);
Point3 normalAx = axis.GetRow(2);
//capsule center
Point3 center = ragdollParent->GetObjOffsetPos();
//min and max points
Point3 pt1 = center - normalAx*(length/2);
Point3 pt2 = center + normalAx*(length/2);
capsule->SetFirstPoint(TOVECTOR3(pt1)/scale);
capsule->SetSecondPoint(TOVECTOR3(pt2)/scale);
capsule->SetMaterial(HAV_MAT_SKIN);
shape = StaticCast<bhkShape>(capsule);
}
else {
// Sphere
//CalcBoundingSphere(node, tm.GetTrans(), radius, 0);
bhkSphereShapeRef sphere = new bhkSphereShape();
sphere->SetRadius(radius/scale);
sphere->SetMaterial(HAV_MAT_SKIN);
shape = StaticCast<bhkShape>(sphere);
}
bhkRigidBodyRef body;
if (shape)
{
bhkBlendCollisionObjectRef blendObj = new bhkBlendCollisionObject();
body = new bhkRigidBody();
Matrix3 tm = ragdollParent->GetObjTMAfterWSM(0);
//Calculate Object Offset Matrix
Matrix3 otm(1);
Point3 pos = ragdollParent->GetObjOffsetPos();
otm.PreTranslate(pos);
Quat quat = ragdollParent->GetObjOffsetRot();
PreRotateMatrix(otm, quat);
Matrix3 otmInvert = otm;
otmInvert.Invert();
//correct object tm
Matrix3 tmbhk = otmInvert * tm;
//set geometric parameters
body->SetRotation(TOQUATXYZW(Quat(tmbhk).Invert()));
body->SetTranslation(TOVECTOR4(tmbhk.GetTrans() / scale));
body->SetCenter(TOVECTOR4(ragdollParent->GetObjOffsetPos())/scale);
//set physics
if (IParamBlock2* rbParameters = rbMod->GetParamBlockByID(PB_RB_MOD_PBLOCK)) {
//These are fundamental parameters
int lyr = NP_DEFAULT_HVK_LAYER;
int mtl = NP_DEFAULT_HVK_MATERIAL;
int msys = NP_DEFAULT_HVK_MOTION_SYSTEM;
int qtype = NP_DEFAULT_HVK_QUALITY_TYPE;
float mass = NP_DEFAULT_HVK_MASS;
float lindamp = NP_DEFAULT_HVK_LINEAR_DAMPING;
float angdamp = NP_DEFAULT_HVK_ANGULAR_DAMPING;
float frict = NP_DEFAULT_HVK_FRICTION;
float maxlinvel = NP_DEFAULT_HVK_MAX_LINEAR_VELOCITY;
float maxangvel = NP_DEFAULT_HVK_MAX_ANGULAR_VELOCITY;
float resti = NP_DEFAULT_HVK_RESTITUTION;
float pendepth = NP_DEFAULT_HVK_PENETRATION_DEPTH;
Point3 InertiaTensor;
rbParameters->GetValue(PA_RB_MOD_MASS, 0, mass, FOREVER);
rbParameters->GetValue(PA_RB_MOD_RESTITUTION, 0, resti, FOREVER);
rbParameters->GetValue(PA_RB_MOD_FRICTION, 0, frict, FOREVER);
rbParameters->GetValue(PA_RB_MOD_INERTIA_TENSOR, 0, InertiaTensor, FOREVER);
rbParameters->GetValue(PA_RB_MOD_LINEAR_DAMPING, 0, lindamp, FOREVER);
rbParameters->GetValue(PA_RB_MOD_CHANGE_ANGULAR_DAMPING, 0, angdamp, FOREVER);
rbParameters->GetValue(PA_RB_MOD_MAX_LINEAR_VELOCITY, 0, maxlinvel, FOREVER);
rbParameters->GetValue(PA_RB_MOD_MAX_ANGULAR_VELOCITY, 0, maxangvel, FOREVER);
rbParameters->GetValue(PA_RB_MOD_ALLOWED_PENETRATION_DEPTH, 0, pendepth, FOREVER);
rbParameters->GetValue(PA_RB_MOD_QUALITY_TYPE, 0, qtype, FOREVER);
body->SetMass(mass);
body->SetRestitution(resti);
body->SetFriction(frict);
body->SetLinearDamping(lindamp);
body->SetMaxLinearVelocity(maxlinvel);
body->SetMaxAngularVelocity(maxangvel);
body->SetPenetrationDepth(pendepth);
InertiaMatrix im;
im[0][0] = InertiaTensor[0];
im[1][1] = InertiaTensor[1];
im[2][2] = InertiaTensor[2];
body->SetInertia(im);
/*switch (qtype) {
case QT_FIXED:
body->SetQualityType(MO_QUAL_FIXED);
break;
case QT_KEYFRAMED:
body->SetQualityType(MO_QUAL_KEYFRAMED);
break;
case QT_DEBRIS:
body->SetQualityType(MO_QUAL_DEBRIS);
break;
case QT_MOVING:
body->SetQualityType(MO_QUAL_MOVING);
break;
case QT_CRITICAL:
body->SetQualityType(MO_QUAL_CRITICAL);
break;
case QT_BULLET:
body->SetQualityType(MO_QUAL_BULLET);
break;
case QT_KEYFRAMED_REPORTING:
body->SetQualityType(MO_QUAL_KEYFRAMED_REPORT);
break;
}*/
body->SetSkyrimLayer(SkyrimLayer::SKYL_BIPED);
body->SetSkyrimLayerCopy(SkyrimLayer::SKYL_BIPED);
body->SetMotionSystem(MotionSystem::MO_SYS_BOX);
body->SetDeactivatorType(DeactivatorType::DEACTIVATOR_NEVER);
body->SetSolverDeactivation(SolverDeactivation::SOLVER_DEACTIVATION_LOW);
body->SetQualityType(MO_QUAL_FIXED);
}
if (constraintMod && ragdollParent->GetParentNode() && parent->GetParent()) {
if (constraintMod->ClassID() == HK_CONSTRAINT_RAGDOLL_CLASS_ID) {
bhkRagdollConstraintRef ragdollConstraint = new bhkRagdollConstraint();
//entities
ragdollConstraint->AddEntity(body);
NiNodeRef parentRef = parent->GetParent();
bhkRigidBodyRef nifParentRigidBody;
while (parentRef) {
if (parentRef->GetCollisionObject()) {
nifParentRigidBody = StaticCast<bhkRigidBody>(StaticCast<bhkBlendCollisionObject>(parentRef->GetCollisionObject())->GetBody());
break;
}
parentRef = parentRef->GetParent();
}
if (!nifParentRigidBody)
throw exception(FormatText("Unable to find NIF constraint parent for ragdoll node %s", ragdollParent->GetName()));
ragdollConstraint->AddEntity(nifParentRigidBody);
RagdollDescriptor desc;
//parameters
if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_CONSTRAINT_MOD_COMMON_SPACES_PARAMS)) {
Point3 pivotA;
Matrix3 parentRotation;
Point3 pivotB;
Matrix3 childRotation;
constraintParameters->GetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_TRANSLATION, 0, pivotB, FOREVER);
constraintParameters->GetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_ROTATION, 0, childRotation, FOREVER);
constraintParameters->GetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_TRANSLATION, 0, pivotA, FOREVER);
constraintParameters->GetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_ROTATION, 0, parentRotation, FOREVER);
desc.pivotA = TOVECTOR4(pivotA);
desc.pivotB = TOVECTOR4(pivotB);
desc.planeA = TOVECTOR4(parentRotation.GetRow(0));
desc.motorA = TOVECTOR4(parentRotation.GetRow(1));
desc.twistA = TOVECTOR4(parentRotation.GetRow(2));
desc.planeB = TOVECTOR4(childRotation.GetRow(0));
desc.motorB = TOVECTOR4(childRotation.GetRow(1));
desc.twistB = TOVECTOR4(childRotation.GetRow(2));
}
if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_RAGDOLL_MOD_PBLOCK)) {
float coneMaxAngle;
float planeMinAngle;
float planeMaxAngle;
float coneMinAngle;
float twistMinAngle;
float maxFriction;
constraintParameters->GetValue(PA_RAGDOLL_MOD_CONE_ANGLE, 0, coneMaxAngle, FOREVER);
constraintParameters->GetValue(PA_RAGDOLL_MOD_PLANE_MIN, 0, planeMinAngle, FOREVER);
constraintParameters->GetValue(PA_RAGDOLL_MOD_PLANE_MAX, 0, planeMaxAngle, FOREVER);
constraintParameters->GetValue(PA_RAGDOLL_MOD_TWIST_MIN, 0, coneMinAngle, FOREVER);
constraintParameters->GetValue(PA_RAGDOLL_MOD_TWIST_MAX, 0, twistMinAngle, FOREVER);
constraintParameters->GetValue(PA_RAGDOLL_MOD_MAX_FRICTION_TORQUE, 0, maxFriction, FOREVER);
desc.coneMaxAngle = TORAD(coneMaxAngle);
desc.planeMinAngle = TORAD(planeMinAngle);
desc.planeMaxAngle = TORAD(planeMaxAngle);
desc.coneMaxAngle = TORAD(coneMinAngle);
desc.twistMinAngle = TORAD(twistMinAngle);
desc.maxFriction = maxFriction;
}
ragdollConstraint->SetRagdoll(desc);
body->AddConstraint(ragdollConstraint);
}
else if (constraintMod->ClassID() == HK_CONSTRAINT_HINGE_CLASS_ID) {
bhkLimitedHingeConstraintRef limitedHingeConstraint = new bhkLimitedHingeConstraint();
//entities
limitedHingeConstraint->AddEntity(body);
NiNodeRef parentRef = parent->GetParent();
bhkRigidBodyRef nifParentRigidBody;
while (parentRef) {
if (parentRef->GetCollisionObject()) {
nifParentRigidBody = StaticCast<bhkRigidBody>(StaticCast<bhkBlendCollisionObject>(parentRef->GetCollisionObject())->GetBody());
break;
}
parentRef = parentRef->GetParent();
}
if (!nifParentRigidBody)
throw exception(FormatText("Unable to find NIF constraint parent for limited hinge node %s", ragdollParent->GetName()));
limitedHingeConstraint->AddEntity(nifParentRigidBody);
LimitedHingeDescriptor lh;
if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_CONSTRAINT_MOD_COMMON_SPACES_PARAMS)) {
Matrix3 parentRotation;
Matrix3 childRotation;
constraintParameters->GetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_ROTATION, 0, childRotation, FOREVER);
constraintParameters->GetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_ROTATION, 0, parentRotation, FOREVER);
lh.perp2AxleInA1 = TOVECTOR4(parentRotation.GetRow(0));
lh.perp2AxleInA2 = TOVECTOR4(parentRotation.GetRow(1));
lh.axleA = TOVECTOR4(parentRotation.GetRow(2));
lh.perp2AxleInB1 = TOVECTOR4(childRotation.GetRow(0));
lh.perp2AxleInB2 = TOVECTOR4(childRotation.GetRow(1));
lh.axleB = TOVECTOR4(childRotation.GetRow(2));
}
if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_HINGE_MOD_PBLOCK)) {
float minAngle;
float maxAngle;
float maxFriction;
constraintParameters->GetValue(PA_HINGE_MOD_LIMIT_MIN, 0, minAngle, FOREVER);
constraintParameters->GetValue(PA_HINGE_MOD_LIMIT_MAX, 0, maxAngle, FOREVER);
constraintParameters->GetValue(PA_HINGE_MOD_MAX_FRICTION_TORQUE, 0, maxFriction, FOREVER);
// constraintParameters->SetValue(PA_HINGE_MOD_MOTOR_TYPE, 0, lh.motor., 0);
lh.minAngle = TORAD(minAngle);
lh.maxAngle = TORAD(maxAngle);
lh.maxAngle = maxFriction;
}
limitedHingeConstraint->SetLimitedHinge(lh);
body->AddConstraint(limitedHingeConstraint);
}
}
//InitializeRigidBody(body, node);
body->SetShape(shape);
blendObj->SetBody(StaticCast<NiObject>(body));
parent->SetCollisionObject(StaticCast<NiCollisionObject>(blendObj));
}
////rigid body parameters
// // get data from node
//int lyr = NP_DEFAULT_HVK_LAYER;
//int mtl = NP_DEFAULT_HVK_MATERIAL;
//int msys = NP_DEFAULT_HVK_MOTION_SYSTEM;
//int qtype = NP_DEFAULT_HVK_QUALITY_TYPE;
//float mass = NP_DEFAULT_HVK_MASS;
//float lindamp = NP_DEFAULT_HVK_LINEAR_DAMPING;
//float angdamp = NP_DEFAULT_HVK_ANGULAR_DAMPING;
//float frict = NP_DEFAULT_HVK_FRICTION;
//float maxlinvel = NP_DEFAULT_HVK_MAX_LINEAR_VELOCITY;
//float maxangvel = NP_DEFAULT_HVK_MAX_ANGULAR_VELOCITY;
//float resti = NP_DEFAULT_HVK_RESTITUTION;
//float pendepth = NP_DEFAULT_HVK_PENETRATION_DEPTH;
//BOOL transenable = TRUE;
//if (IParamBlock2* rbParameters = rbMod->GetParamBlockByID(PB_SHAPE_MOD_PBLOCK))
//{
// //These are fundamental parameters
// rbParameters->GetValue(PA_RB_MOD_MASS, 0, mass, FOREVER);
// rbParameters->GetValue(PA_RB_MOD_RESTITUTION, 0, resti, FOREVER);
// rbParameters->GetValue(PA_RB_MOD_FRICTION, 0, frict, FOREVER);
// rbParameters->GetValue(PA_RB_MOD_LINEAR_DAMPING, 0, lindamp, FOREVER);
// rbParameters->GetValue(PA_RB_MOD_CHANGE_ANGULAR_DAMPING, 0, angdamp, FOREVER);
// rbParameters->GetValue(PA_RB_MOD_MAX_LINEAR_VELOCITY, 0, maxlinvel, FOREVER);
// rbParameters->GetValue(PA_RB_MOD_MAX_ANGULAR_VELOCITY, 0, maxangvel, FOREVER);
// rbParameters->GetValue(PA_RB_MOD_ALLOWED_PENETRATION_DEPTH, 0, pendepth, FOREVER);
// rbParameters->GetValue(PA_RB_MOD_QUALITY_TYPE, 0, qtype, FOREVER);
// switch (qtype) {
// case MO_QUAL_INVALID:
// break;
// case QT_FIXED:
// rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, MO_QUAL_FIXED, 0);
// break;
// case MO_QUAL_KEYFRAMED:
// rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_KEYFRAMED, 0);
// break;
// case MO_QUAL_DEBRIS:
// rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_DEBRIS, 0);
// break;
// case MO_QUAL_MOVING:
// rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_MOVING, 0);
// break;
// case MO_QUAL_CRITICAL:
// rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_CRITICAL, 0);
// break;
// case MO_QUAL_BULLET:
// rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_BULLET, 0);
// break;
// case MO_QUAL_USER:
// break;
// case MO_QUAL_CHARACTER:
// break;
// case MO_QUAL_KEYFRAMED_REPORT:
// rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_KEYFRAMED_REPORTING, 0);
// break;
// }
// // setup body
// bhkRigidBodyRef body = transenable ? new bhkRigidBodyT() : new bhkRigidBody();
// OblivionLayer obv_layer; SkyrimLayer sky_layer;
// GetHavokLayersFromIndex(lyr, (int*)&obv_layer, (int*)&sky_layer);
// body->SetLayer(obv_layer);
// body->SetLayerCopy(obv_layer);
// body->SetSkyrimLayer(sky_layer);
// body->SetMotionSystem(MotionSystem(msys));
// body->SetQualityType(MotionQuality(qtype));
// body->SetMass(mass);
// body->SetLinearDamping(lindamp);
// body->SetAngularDamping(angdamp);
// body->SetFriction(frict);
// body->SetRestitution(resti);
// body->SetMaxLinearVelocity(maxlinvel);
// body->SetMaxAngularVelocity(maxangvel);
// body->SetPenetrationDepth(pendepth);
// body->SetCenter(center);
// QuaternionXYZW q; q.x = q.y = q.z = 0; q.w = 1.0f;
// body->SetRotation(q);
//}
}
int ico_dot_dod(int densit) /* densit...required dots per unit sphere */
{
/* dot distribution on a unit sphere based on an icosaeder *
* great circle average refining of icosahedral face */
int i, j, k, tl, tl2, tn, tess, j1, j2;
real a, d, x, y, z, x2, y2, z2, x3, y3, z3, ai_d, adod;
real xij, yij, zij, xji, yji, zji, xik, yik, zik, xki, yki, zki,
xjk, yjk, zjk, xkj, ykj, zkj;
real *xus = NULL;
/* calculate tesselation level */
a = sqrt((((real) densit)-2.)/30.);
tess = max((int) ceil(a), 1);
n_dot = 30*tess*tess+2;
if (n_dot < densit)
{
ERROR("ico_dot_dod: error in formula for tessalation level (%d->%d, %d)",
tess, n_dot, densit);
}
snew(xus, 3*n_dot);
xpunsp = xus;
icosaeder_vertices(xus);
tn = 12;
/* square of the edge of an icosaeder */
a = rh*rh*2.*(1.-cos(TORAD(72.)));
/* dodecaeder vertices */
for (i = 0; i < 10; i++)
{
for (j = i+1; j < 11; j++)
{
x = xus[3*i]-xus[3*j];
y = xus[1+3*i]-xus[1+3*j]; z = xus[2+3*i]-xus[2+3*j];
d = x*x+y*y+z*z;
if (fabs(a-d) > DP_TOL)
{
continue;
}
for (k = j+1; k < 12; k++)
{
x = xus[3*i]-xus[3*k];
y = xus[1+3*i]-xus[1+3*k]; z = xus[2+3*i]-xus[2+3*k];
d = x*x+y*y+z*z;
if (fabs(a-d) > DP_TOL)
{
continue;
}
x = xus[3*j]-xus[3*k];
y = xus[1+3*j]-xus[1+3*k]; z = xus[2+3*j]-xus[2+3*k];
d = x*x+y*y+z*z;
if (fabs(a-d) > DP_TOL)
{
continue;
}
x = xus[ 3*i]+xus[ 3*j]+xus[ 3*k];
y = xus[1+3*i]+xus[1+3*j]+xus[1+3*k];
z = xus[2+3*i]+xus[2+3*j]+xus[2+3*k];
d = sqrt(x*x+y*y+z*z);
xus[3*tn] = x/d; xus[1+3*tn] = y/d; xus[2+3*tn] = z/d;
tn++;
}
}
}
if (tess > 1)
{
tn = 32;
/* square of the edge of an dodecaeder */
adod = 4.*(cos(TORAD(108.))-cos(TORAD(120.)))/(1.-cos(TORAD(120.)));
/* square of the distance of two adjacent vertices of ico- and dodecaeder */
ai_d = 2.*(1.-sqrt(1.-a/3.));
/* calculate tessalation of mixed edges */
for (i = 0; i < 31; i++)
{
j1 = 12; j2 = 32; a = ai_d;
if (i >= 12)
{
j1 = i+1; a = adod;
}
for (j = j1; j < j2; j++)
{
x = xus[3*i]-xus[3*j];
y = xus[1+3*i]-xus[1+3*j]; z = xus[2+3*i]-xus[2+3*j];
d = x*x+y*y+z*z;
if (fabs(a-d) > DP_TOL)
{
continue;
}
for (tl = 1; tl < tess; tl++)
{
if (tn >= n_dot)
{
ERROR("ico_dot: tn exceeds dimension of xus");
}
divarc(xus[3*i], xus[1+3*i], xus[2+3*i],
xus[3*j], xus[1+3*j], xus[2+3*j],
tl, tess, &xus[3*tn], &xus[1+3*tn], &xus[2+3*tn]);
tn++;
}
}
}
/* calculate tessalation of pentakisdodecahedron faces */
for (i = 0; i < 12; i++)
{
for (j = 12; j < 31; j++)
{
x = xus[3*i]-xus[3*j];
y = xus[1+3*i]-xus[1+3*j]; z = xus[2+3*i]-xus[2+3*j];
d = x*x+y*y+z*z;
if (fabs(ai_d-d) > DP_TOL)
{
continue;
}
for (k = j+1; k < 32; k++)
{
x = xus[3*i]-xus[3*k];
y = xus[1+3*i]-xus[1+3*k]; z = xus[2+3*i]-xus[2+3*k];
d = x*x+y*y+z*z;
if (fabs(ai_d-d) > DP_TOL)
{
continue;
}
x = xus[3*j]-xus[3*k];
y = xus[1+3*j]-xus[1+3*k]; z = xus[2+3*j]-xus[2+3*k];
d = x*x+y*y+z*z;
if (fabs(adod-d) > DP_TOL)
{
continue;
}
for (tl = 1; tl < tess-1; tl++)
{
divarc(xus[3*j], xus[1+3*j], xus[2+3*j],
xus[3*i], xus[1+3*i], xus[2+3*i],
tl, tess, &xji, &yji, &zji);
divarc(xus[3*k], xus[1+3*k], xus[2+3*k],
xus[3*i], xus[1+3*i], xus[2+3*i],
tl, tess, &xki, &yki, &zki);
for (tl2 = 1; tl2 < tess-tl; tl2++)
{
divarc(xus[3*i], xus[1+3*i], xus[2+3*i],
xus[3*j], xus[1+3*j], xus[2+3*j],
tl2, tess, &xij, &yij, &zij);
divarc(xus[3*k], xus[1+3*k], xus[2+3*k],
xus[3*j], xus[1+3*j], xus[2+3*j],
tl2, tess, &xkj, &ykj, &zkj);
divarc(xus[3*i], xus[1+3*i], xus[2+3*i],
xus[3*k], xus[1+3*k], xus[2+3*k],
tess-tl-tl2, tess, &xik, &yik, &zik);
divarc(xus[3*j], xus[1+3*j], xus[2+3*j],
xus[3*k], xus[1+3*k], xus[2+3*k],
tess-tl-tl2, tess, &xjk, &yjk, &zjk);
if (tn >= n_dot)
{
ERROR("ico_dot: tn exceeds dimension of xus");
}
divarc(xki, yki, zki, xji, yji, zji, tl2, tess-tl,
&x, &y, &z);
divarc(xkj, ykj, zkj, xij, yij, zij, tl, tess-tl2,
&x2, &y2, &z2);
divarc(xjk, yjk, zjk, xik, yik, zik, tl, tl+tl2,
&x3, &y3, &z3);
x = x+x2+x3; y = y+y2+y3; z = z+z2+z3;
d = sqrt(x*x+y*y+z*z);
xus[3*tn] = x/d;
xus[1+3*tn] = y/d;
xus[2+3*tn] = z/d;
tn++;
} /* cycle tl2 */
} /* cycle tl */
} /* cycle k */
} /* cycle j */
} /* cycle i */
if (n_dot != tn)
{
ERROR("ico_dot: n_dot(%d) and tn(%d) differ", n_dot, tn);
}
} /* end of if (tess > 1) */
return n_dot;
} /* end of routine ico_dot_dod */
