public copy()
{
String modelNameA = CSMGEN.RD.next();
String modelNameB = CSMGEN.RD.next();
CSMGEN.PR.WriteLine("Copy: {0} -> {1}\n", modelNameA, modelNameB);
if (modelNameA.Equals(modelNameB))
{
return;
}
if (CSMGEN.blocks.ContainsKey(modelNameA))
{
mA = (FeModel)CSMGEN.blocks[modelNameA];
}
else
{
UTIL.errorMsg("No such mesh block: " + modelNameA);
}
FeModel mB = copyMesh();
CSMGEN.blocks.Add(modelNameB, mB);
CSMGEN.PR.WriteLine("Mesh " + modelNameB + ": nEl = {0,9} nNod = {1,9}\n", mB.nEl, mB.nNod);
}
private void doMirror()
{
if (m.nDim == 2 && axis == 'z')
{
return;
}
int iax = getIntAxis(axis);
// Mirror nodal coordinates
for (int i = 0; i < m.nNod; i++)
{
m.setNodeCoord(i, iax, -m.getNodeCoord(i, iax) + 2 * value);
}
// Change order of element connectivities
for (int e = 0; e < m.nEl; e++)
{
elementMirror em = elementMirror.Null;
try
{
//em = elementMirror.valueOf(m.elems[e].name);
em = (elementMirror)System.Enum.Parse(typeof(elementMirror), m.elems[e].name);
}
catch (Exception el)
{
UTIL.errorMsg("Mirror: element not supported " + m.elems[e].name);
}
int nind = m.elems[e].ind.Length;
int[] ind = new int[nind];
for (int i = 0; i < nind; i++)
{
ind[em.permutation(i)] = m.elems[e].ind[i];
}
m.elems[e].setElemConnectivities(ind);
}
}
protected void Page_Load(object sender, EventArgs e)
{
try
{
UserManager UM = new UserManager();
UserTBx user = new UserTBx();
int id = Convert.ToInt32(Request["id"]);
user = UM.GetByID(id);
user.status = 1;
user.token = "";
user.typeuser_id = Convert.ToInt32(Request["typeuser"]);
user.first_name = Request["firstname"];
user.last_name = Request["lastname"];
user.full_name = Request["fullname"];
user.email = Request["email"];
user.password = UTIL.Encrypt(Request["password"], true);
user.phone = Request["phone"];
user.birthday = Request["birthday"];
user.address = Request["address"];
user.credit = Convert.ToInt32(Request["credit"]);
user.create_day = user.create_day;
user.last_loginday = "";
UM.Save();
Response.Write(JsonConvert.SerializeObject(new
{
success = 1
}));
}
catch (Exception ex)
{
Response.Write(JsonConvert.SerializeObject(new
{
success = -1,
error = ex
}));
}
}
// Create data for constrained displacements
// specified inside a box
private void createBoxConstrDisplacements(FeScanner es)
{
String s = es.next().ToLower();
int idf = UTIL.direction(s);
if (idf == -1)
{
UTIL.errorMsg("boxConstrDispl direction should be x/y/z. Specified:" + s);
}
if (!es.hasNextDouble())
{
UTIL.errorMsg("boxConstrDispl value is not a double: " + es.next());
}
double vd = es.nextDouble();
for (int i = 0; i < 2; i++)
{
for (int j = 0; j < nDim; j++)
{
box[i][j] = es.readDouble();
}
}
//NODE:
for (int i = 0; i < nNod; i++)
{
for (int j = 0; j < nDim; j++)
{
double x = getNodeCoord(i, j);
if (x < box[0][j] || x > box[1][j])
{
goto NODE;
}
}
defDs.Add(new Dof(nDim * i + idf, vd));
NODE : { }
}
}
// Set displacement differentiation matrix bmat.
// xi, et - local coordinates,
// returns determinant of Jacobian matrix
private double setBmatrix(double xi, double et)
{
// Derivatives of shape functions
double det = ShapeQuad2D.deriv(xi, et, ind, xy, dnxy);
if (det <= 0)
{
UTIL.errorMsg("Negative/zero 8Nr element area");
}
if (FeModel.stressState == FeModel.StrStates.axisym)
{
ShapeQuad2D.shape(xi, et, ind, an);
r = 0.0;
for (int i = 0; i < 8; i++)
{
r += an[i] * xy[i][0];
}
}
// Eight blocks of the displacement differentiation
// matrix
for (int ib = 0; ib < 8; ib++)
{
bmat[0][2 * ib] = dnxy[ib][0];
bmat[0][2 * ib + 1] = 0.0;
bmat[1][2 * ib] = 0.0;
bmat[1][2 * ib + 1] = dnxy[ib][1];
bmat[2][2 * ib] = dnxy[ib][1];
bmat[2][2 * ib + 1] = dnxy[ib][0];
if (FeModel.stressState == FeModel.StrStates.axisym)
{
bmat[3][2 * ib] = an[ib] / r;
bmat[3][2 * ib + 1] = 0.0;
}
}
return(det);
}
// Derivatives of shape functions with respect to global coordinates x.
// xi - local coordinates;
// xy[2][3] - nodal coordinates;
// dnxy[2][1] - derivatives of shape functions (out);
// returns determinant of the Jacobian matrrix
public static double deriv(double xi, double[][] xy, double[] dnxy)
{
// Derivatives in local coords dN/dXi
double dnxe1 = -0.5;
double dnxe2 = 0.5;
// Jacobian
L = Math.Sqrt(Math.Pow((xy[1][0] - xy[0][0]), 2.0) + Math.Pow((xy[1][1] - xy[0][1]), 2.0) + Math.Pow((xy[1][2] - xy[0][2]), 2.0));
double det = L / 2.0;
// Zero or negative determinant
if (det <= 0)
{
UTIL.errorMsg("Negative/zero Jacobian determinant for 2N element " + (float)det);
}
// Jacobian inverse
double aj00 = 1.0 / det;
// Derivatives in global coordinates dN/dx
dnxy[0] = aj00 * dnxe1;
dnxy[1] = aj00 * dnxe2;
return(det);
}
private void readDataFile(FeScanner es)
{
vars name = vars.NONE;
String s;
Material mat;
it = defDs.GetEnumerator();
while (es.hasNext())
{
varName = es.next();
String varname = varName.ToLower();
if (varname.Equals("#"))
{
es.nextLine(); continue;
}
try {
name = (vars)System.Enum.Parse(typeof(vars), varname);//vars.valueOf(varname);
//Console.WriteLine("hmm");
} catch (Exception E) {
UTIL.errorMsg("Variable name is not found: " + varName);
}
switch (name)
{
case vars.nel:
nEl = es.readInt();
//Console.WriteLine("FEMODEL: "+nEl.ToString());
break;
case vars.nnod:
nNod = es.readInt();
//Console.WriteLine("FEMODEL: " + nNod.ToString());
break;
case vars.ndim:
nDim = es.readInt();
nDf = nDim;
//Console.WriteLine("FEMODEL: " + nDf.ToString());
break;
case vars.stressstate:
s = es.next().ToLower();
try {
stressState = (StrStates)System.Enum.Parse(typeof(StrStates), s); //StrStates.valueOf(s);
} catch (Exception E) {
UTIL.errorMsg("stressState has forbidden value: " + s);
}
if (stressState != StrStates.threed)
{
nDim = nDf = 2;
}
else
{
nDim = nDf = 3;
}
//Console.WriteLine("FEMODEL: " + stressState.ToString());
break;
case vars.physlaw:
s = es.next().ToLower();
try {
physLaw = (PhysLaws)System.Enum.Parse(typeof(PhysLaws), s); //PhysLaws.valueOf(s);
} catch (Exception E) {
UTIL.errorMsg("physLaw has forbidden value: " + s);
}
//Console.WriteLine("FEMODEL: " + s.ToString());
break;
case vars.solver:
s = es.next().ToLower();
try {
Solver.solver = (Solver.solvers)System.Enum.Parse(typeof(Solver.solvers), s); //Solver.solvers.valueOf(s);
} catch (Exception E) {
UTIL.errorMsg("solver has forbidden value: " + s);
}
//Console.WriteLine("FEMODEL: " + s.ToString());
break;
case vars.elcon:
readElemData(es);
break;
case vars.nodcoord:
if (nNod == 0 || nDim == 0)
{
UTIL.errorMsg("nNod and nDim should be specified before nodCoord");
}
nEq = nNod * nDim;
// Nodal coordinates
newCoordArray();
for (int i = 0; i < nNod; i++)
{
for (int j = 0; j < nDim; j++)
{
setNodeCoord(i, j, es.readDouble());
}
}
break;
case vars.material:
String matname = es.next();
mat = Material.newMaterial(physLaw.ToString(), stressState.ToString());
double e = es.readDouble();
double nu = es.readDouble();
double alpha = es.readDouble();
mat.setElasticProp(e, nu, alpha);
if (physLaw == PhysLaws.elplastic)
{
double sY = es.readDouble();
double km = es.readDouble();
double mm = es.readDouble();
mat.setPlasticProp(sY, km, mm);
}
materials.Add(matname, mat);
//Console.WriteLine("FEMODEL: " + matname);
break;
case vars.constrdispl:
readConstrDisplacements(es);
break;
case vars.boxconstrdispl:
createBoxConstrDisplacements(es);
break;
case vars.thermalloading:
s = es.next();
if (s.ToLower().Equals("y"))
{
thermalLoading = true;
}
else if (s.ToLower().Equals("n"))
{
thermalLoading = false;
}
else
{
UTIL.errorMsg("thermalLoading should be y/n. Specified: " + s);
}
break;
case vars.includefile:
s = es.next();
FeScanner R = new FeScanner(s);
readDataFile(R);
break;
case vars.trussarea:
readTrussArea(es);
break;
case vars.end:
return;
}
}
}
private void readData()
{
String varName, varname;
while (CSMGEN.RD.hasNext())
{
varName = CSMGEN.RD.next();
varname = varName.ToLower();
if (varName.Equals("#"))
{
CSMGEN.RD.nextLine(); continue;
}
try
{
//name = vars.valueOf(varname);
name = (vars)System.Enum.Parse(typeof(vars), varname);
}
catch (Exception e)
{
UTIL.errorMsg("Variable name is not found: " + varName);
}
switch (name)
{
case vars.nh:
nh = CSMGEN.RD.readInt();
break;
case vars.nv:
nv = CSMGEN.RD.readInt();
break;
case vars.res:
for (int i = 0; i < 4; i++)
{
res[i] = CSMGEN.RD.readDouble();
}
break;
case vars.xyp:
for (int i = 0; i < 8; i++)
{
xp[i] = CSMGEN.RD.readDouble();
yp[i] = CSMGEN.RD.readDouble();
}
// Interpolation of macroelement midside
// nodes if both coordinates are zeroes
for (int i = 1; i < 8; i += 2)
{
if (xp[i] == 0.0 && yp[i] == 0.0)
{
xp[i] = 0.5 * (xp[i - 1] + xp[(i + 1) % 8]);
yp[i] = 0.5 * (yp[i - 1] + yp[(i + 1) % 8]);
}
}
break;
case vars.mat:
mat = CSMGEN.RD.next();
break;
case vars.thick:
t = CSMGEN.RD.readDouble();
break;
case vars.end:
return;
}
}
}
public void SetAimDirection(Vector3 dir)
{
startingAngle = UTIL.GetAngleFromVectorFloat(dir) + fov / 2f - 180f;
}
public CSFEM()
{
UTIL.printDate(PR);
FeModel fem = new FeModel(RD, PR);
Element.fem = fem;
fem.readData();
//Console.WriteLine(fem.nEl);
PR.Write(Environment.NewLine + "Number of elements nEl = {0,9}" + Environment.NewLine +
"Number of nodes nNod = {1,9}" + Environment.NewLine +
"Number of dimensions nDim = {2,9}" + Environment.NewLine,
fem.nEl, fem.nNod, fem.nDim);
long t0 = Environment.TickCount;
Solver solver = Solver.newSolver(fem);
solver.assembleGSM();
PR.WriteLine("Memory for global matrix: {0,9:0.00} MB" + Environment.NewLine, Solver.lengthOfGSM * 8.0e-6);
FeLoad load = new FeLoad(fem);
Element.load = load;
FeStress stress = new FeStress(fem);
// Load step loop
while (load.readData())
{
load.assembleRHS();
int iter = 0;
// Equilibrium iterations
do
{
iter++;
int its = solver.solve(FeLoad.RHS);
if (its > 0)
{
PR.Write(Environment.NewLine + "Solver: {0} iterations" + Environment.NewLine, its);
}
stress.computeIncrement();
} while (!stress.equilibrium(iter));
stress.accumulate();
stress.writeResults();
PR.WriteLine("Loadstep {0}", FeLoad.loadStepName);
if (iter > 1)
{
PR.WriteLine("{0,5} iterations, " + "Relative residual norm = {1,9:E6}", iter, FeStress.relResidNorm);
}
PR.Write(Environment.NewLine);
}
PR.Write(Environment.NewLine + "Solution time = {0:0.00} s" + Environment.NewLine, (Environment.TickCount - t0) * 0.001);
}
static void readDataFile(FeScanner RD)
{
vars name = vars.none;
while (RD.hasNext())
{
String varName = RD.next();
String varNameLower = varName.ToLower();
if (varName.Equals("#"))
{
RD.nextLine(); continue;
}
try
{
name = (vars)System.Enum.Parse(typeof(vars), varNameLower);
}
catch (Exception e)
{
UTIL.errorMsg("Variable name is not found: " + varName);
}
switch (name)
{
case vars.meshfile:
meshFile = RD.next();
break;
case vars.resultfile:
resultFile = RD.next();
break;
case vars.parm:
try
{
varName = RD.next();
parm = (parms)System.Enum.Parse(typeof(parms), varName.ToLower());
}
catch (Exception e)
{ UTIL.errorMsg("No such result parameter: " + varName); }
break;
case vars.showedges:
showEdges = RD.next().Equals("y");
break;
case vars.shownodes:
showNodes = RD.next().Equals("y");
break;
case vars.ndivmin:
nDivMin = RD.readInt();
break;
case vars.ndivmax:
nDivMax = RD.readInt();
break;
case vars.fmin:
fMin = RD.readDouble();
break;
case vars.fmax:
fMax = RD.readDouble();
break;
case vars.ncontours:
nContours = RD.readInt();
break;
case vars.deformscale:
deformScale = RD.readDouble();
break;
case vars.end:
return;
default:
return;
}
}
}
// Compute stiffness matrix
public override void stiffnessMatrix()
{
for (int i = 0; i < 60; i++)
{
for (int j = i; j < 60; j++)
{
kmat[i][j] = 0.0;
}
}
// Material mat
mat = (Material)fem.materials[matName];
if (mat == null)
{
UTIL.errorMsg("Element material name: " + matName);
}
double lambda = mat.getLambda();
double mu = mat.getMu();
double beta = lambda + 2 * mu;
for (int ip = 0; ip < gk.nIntPoints; ip++)
{
double det = ShapeQuad3D.deriv(gk.xii[ip], gk.eti[ip], gk.zei[ip], ind, xy, dnxy);
double dv = det * gk.wi[ip];
// Upper symmetrical part of the matrix by rows
for (int i = 0; i < 20; i++) // i = row
// dNi/dx, dNi/dy, dNi/dz
{
double dix = dnxy[i][0];
double diy = dnxy[i][1];
double diz = dnxy[i][2];
for (int j = i; j < 20; j++) // j = column
// dNj/dx, dNj/dy, dNj/dz
{
double djx = dnxy[j][0];
double djy = dnxy[j][1];
double djz = dnxy[j][2];
kmat[i * 3][j * 3] += (beta * dix * djx
+ mu * (diy * djy + diz * djz)) * dv;
kmat[i * 3][j * 3 + 1] += (lambda * dix * djy
+ mu * diy * djx) * dv;
kmat[i * 3][j * 3 + 2] += (lambda * dix * djz
+ mu * diz * djx) * dv;
if (j > i)
{
kmat[i * 3 + 1][j * 3]
+= (lambda * diy * djx + mu * dix * djy) * dv;
}
kmat[i * 3 + 1][j * 3 + 1] += (beta * diy * djy
+ mu * (diz * djz + dix * djx)) * dv;
kmat[i * 3 + 1][j * 3 + 2] += (lambda * diy * djz
+ mu * diz * djy) * dv;
if (j > i)
{
kmat[i * 3 + 2][j * 3]
+= (lambda * diz * djx + mu * dix * djz) * dv;
kmat[i * 3 + 2][j * 3 + 1]
+= (lambda * diz * djy + mu * diy * djz) * dv;
}
kmat[i * 3 + 2][j * 3 + 2] += (beta * diz * djz
+ mu * (dix * djx + diy * djy)) * dv;
}
}
}
}
private void drive(List <Robot> robots, List <Ball> balls, float turnAxis, float powerAxis, float strafeAxis)
{
double tempH = Math.Sqrt(powerAxis * powerAxis + strafeAxis * strafeAxis);
drivePID.setDesiredValue(tempH);
power += drivePID.calcPID(power);
if (tempH == 0)
{
double theta = UTIL.normalizeDirection(-rotation + Math.Atan2(velocity.Y, velocity.X));
//powerAxis = (velocity.Y > 0 && UTIL.normalizeDirection(rotation) < Math.PI || velocity.Y < 0 && UTIL.normalizeDirection( rotation) > Math.PI) ? -1 : 1;
powerAxis = (float)(-velocity.Length() * Math.Cos(theta));
strafeAxis = (float)(velocity.Length() * Math.Sin(theta));
//strafeAxis = (float)(velocity.X * Math.Sin(theta));
tempH = velocity.Length();
}
powerAxis = (float)(power * powerAxis / tempH);
strafeAxis = (float)(power * strafeAxis / tempH);
this.previousPosition = location;
powerAxis *= -1;
Vector2 tempLocation = location;
float tempRotation = rotation;
turnPID.setDesiredValue(turnAxis);
angularMomentum += (float)turnPID.calcPID(angularMomentum);
tempRotation = rotation + angularMomentum * turnConst;// (float)turnPID.calcPID(rotation);
if (driveMode == ArcadeDrive)
{
tempLocation = location + new Vector2(powerAxis * ArcadeDriveConstant * (float)Math.Cos(rotation), powerAxis * ArcadeDriveConstant * (float)Math.Sin(rotation));
}
else if (driveMode == FieldCentric)
{
tempLocation = location + new Vector2(strafeAxis * McCannumDriveConstant, -powerAxis * McCannumDriveConstant);
}
else if (driveMode == McCannumDrive)
{
tempLocation = location + new Vector2(-strafeAxis * (float)Math.Cos(rotation - Math.PI / 2) * McCannumDriveConstant, -strafeAxis * (float)Math.Sin(rotation - Math.PI / 2) * McCannumDriveConstant);
tempLocation += new Vector2(powerAxis * McCannumDriveConstant * (float)Math.Cos(rotation), powerAxis * McCannumDriveConstant * (float)Math.Sin(rotation));
}
else if (driveMode == UnicornDrive)
{
tempLocation = location + new Vector2(-strafeAxis * (float)Math.Cos(rotation - Math.PI / 2) * UnicornDriveConstant, -strafeAxis * (float)Math.Sin(rotation - Math.PI / 2) * UnicornDriveConstant);
tempLocation += new Vector2(powerAxis * UnicornDriveConstant * (float)Math.Cos(rotation), powerAxis * UnicornDriveConstant * (float)Math.Sin(rotation));
}
if (tempLocation.X > minXPosition * widthScale && tempLocation.X < maxXPosition * widthScale && tempLocation.Y > minYPosition * heightScale && tempLocation.Y < maxYPosition * heightScale)
{
velocity = tempLocation - location;
foreach (Robot r in robots)
{
if (!r.Equals(this))
{
if (UTIL.distance(r.getLocation(), location) <= Math.Sqrt(robotImage.Width * robotImage.Width * scale.X * scale.X + robotImage.Height * robotImage.Height * scale.Y * scale.Y) * .8f)
{
if (r.pushRobot(location, velocity, robots) == 1)
{
velocity /= 2f;
}
else if (r.pushRobot(location, velocity, robots) == 0)
{
velocity *= 0f;
}
}
}
}
if (velocity.Length() != 0)
{
driveSoundInstance.Play();
}
else
{
driveSoundInstance.Stop();
}
location += velocity;
}
rotation = tempRotation;
foreach (Ball b in balls)
{
if (!b.Equals(activeBall) && UTIL.distance(location, b.getLocation()) < Ball.radius / 2 && b.getHeight() < 1f && b.getIsFree())
{
if ((driverInput.getLeftTrigger() > .5 || (CPU && this.color.Equals(b.getColor()) && aiHandler.get().getType() != AICommand.defenseCommand && aiHandler.get().getType() != AICommand.driveCommand)) && Math.Abs(UTIL.normalizeDirection(rotation) - UTIL.normalizeDirection(UTIL.getDirectionTward(location, b.getLocation()))) < .6 && activeBall == null)
{
feed.Play();
b.linkRobot(this);
activeBall = b;
if (b.getColor().Equals(this.color))
{
penaltyStart = 0.0;
}
else
{
penaltyStart = time;
}
}
else
{
b.pushBall(UTIL.magD(velocity.Length() * 2.5f + 1, UTIL.getDirectionTward(Vector2.Zero, velocity)));
}
}
}
}
// Compute stiffness matrix
public override void stiffnessMatrix()
{
// Transformation matrix
L = Math.Sqrt(Math.Pow((xy[1][0] - xy[0][0]), 2.0) + Math.Pow((xy[1][1] - xy[0][1]), 2.0));
double lij = (xy[1][0] - xy[0][0]) / L;
double mij = (xy[1][1] - xy[0][1]) / L;
tmat[0][0] = tmat[1][2] = lij;
tmat[0][1] = tmat[1][3] = mij;
// Zeros to stiffness matrix kmat
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
kmat[i][j] = 0.0;
}
}
double[][] kmat_lcl = new double[2][] { new double[2] {
0, 0
}, new double[2] {
0, 0
} };
// ld = length of strain/stress vector (1)
int ld = 1;
// Material mat
mat = (Material)fem.materials[matName];
//Console.WriteLine(matName);
if (mat == null)
{
UTIL.errorMsg("Element material name: " + matName);
}
mat.elasticityMatrix(emat);
// Gauss integration loop
for (int ip = 0; ip < gk.nIntPoints; ip++)
{
// Set displacement differentiation matrix bmat
double det = setBmatrix(gk.xii[ip]);
double dv = det * gk.wi[ip];
for (int i = 0; i < 2; i++)
{
for (int j = 0; j < 2; j++)
{
double s = 0.0;
for (int k = 0; k < ld; k++)
{
for (int l = 0; l < ld; l++)
{
s += bmat[l][i] * mat.getElasticModulus() * bmat[k][j] * this.A;
}
}
kmat_lcl[i][j] += s * dv;
}
}
}
// Apply Transformation and create upper symmetrical part of the stiffness matrix
for (int i = 0; i < 4; i++)
{
for (int j = i; j < 4; j++)
{
for (int k = 0; k < 2; k++)
{
for (int l = 0; l < 2; l++)
{
kmat[i][j] += tmat[l][i] * kmat_lcl[l][k] * tmat[k][j];
}
}
}
}
}
// Derivatives of shape functions
// with respect to global coordinates xy.
// xi, et, ze - local coordinates;
// ind - element connectivities;
// xy - nodal coordinates;
// dnxy - derivatives of shape functions (out);
// returns determinant of the Jacobian matrrix
public static double deriv(double xi, double et, double ze, int[] ind, double[][] xy, double[][] dnxy)
{
// Derivatives with respect to local coordinates d
double[][] d = new double[8][];
for (int i = 0; i < 8; i++)
{
d[i] = new double[3];
}
double s0 = 1 + xi;
double t0 = 1 + et;
double d0 = 1 + ze;
double s1 = 1 - xi;
double t1 = 1 - et;
double d1 = 1 - ze;
// Vertex nodes
d[0][0] = -0.125 * t1 * d1;
d[0][1] = -0.125 * s1 * d1;
d[0][2] = -0.125 * s1 * t1;
d[1][0] = 0.125 * t1 * d1;
d[1][1] = -0.125 * s0 * d1;
d[1][2] = -0.125 * s0 * t1;
d[2][0] = 0.125 * t0 * d1;
d[2][1] = 0.125 * s0 * d1;
d[2][2] = -0.125 * s0 * t0;
d[3][0] = -0.125 * t0 * d1;
d[3][1] = 0.125 * s1 * d1;
d[3][2] = -0.125 * s1 * t0;
d[4][0] = -0.125 * t1 * d0;
d[4][1] = -0.125 * s1 * d0;
d[4][2] = 0.125 * s1 * t1;
d[5][0] = 0.125 * t1 * d0;
d[5][1] = -0.125 * s0 * d0;
d[5][2] = 0.125 * s0 * t1;
d[6][0] = 0.125 * t0 * d0;
d[6][1] = 0.125 * s0 * d0;
d[6][2] = 0.125 * s0 * t0;
d[7][0] = -0.125 * t0 * d0;
d[7][1] = 0.125 * s1 * d0;
d[7][2] = 0.125 * s1 * t0;
// Modification of derivatives due to degeneration
if (degeneration(ind) == 1)
{
for (int i = 0; i < 3; i++)
{
double i1 = d[0][i] + d[3][i];
d[0][i] = d[3][i] = i1;
double i2 = d[4][i] + d[7][i];
d[4][i] = d[7][i] = i2;
}
}
// Jacobian matrix ja
double[][] ja = new double[3][] { new double[3], new double[3], new double[3] };
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
ja[j][i] = 0.0;
for (int k = 0; k < 8; k++)
{
ja[j][i] += d[k][i] * xy[k][j];
}
}
}
// Determinant of Jacobian matrix det
double det =
ja[0][0] * (ja[1][1] * ja[2][2] - ja[2][1] * ja[1][2])
- ja[1][0] * (ja[0][1] * ja[2][2] - ja[0][2] * ja[2][1])
+ ja[2][0] * (ja[0][1] * ja[1][2] - ja[0][2] * ja[1][1]);
if (det <= 0)
{
UTIL.errorMsg("Negative/zero Jacobian determinant for 8N element " + (float)det);
}
// Jacobian inverse ja1
double[][] ja1 = new double[3][] { new double[3], new double[3], new double[3] };
double v = 1.0 / det;
ja1[0][0] = (ja[1][1] * ja[2][2] - ja[2][1] * ja[1][2]) * v;
ja1[1][0] = (ja[0][2] * ja[2][1] - ja[0][1] * ja[2][2]) * v;
ja1[2][0] = (ja[0][1] * ja[1][2] - ja[0][2] * ja[1][1]) * v;
ja1[0][1] = (ja[1][2] * ja[2][0] - ja[1][0] * ja[2][2]) * v;
ja1[1][1] = (ja[0][0] * ja[2][2] - ja[2][0] * ja[0][2]) * v;
ja1[2][1] = (ja[0][2] * ja[1][0] - ja[0][0] * ja[1][2]) * v;
ja1[0][2] = (ja[2][1] * ja[1][0] - ja[2][0] * ja[1][1]) * v;
ja1[1][2] = (ja[0][1] * ja[2][0] - ja[0][0] * ja[2][1]) * v;
ja1[2][2] = (ja[0][0] * ja[1][1] - ja[1][0] * ja[0][1]) * v;
for (int k = 0; k < 8; k++)
{
for (int i = 0; i < 3; i++)
{
dnxy[k][i] = 0.0;
for (int j = 0; j < 3; j++)
{
dnxy[k][i] += ja1[i][j] * d[k][j];
}
}
}
return(det);
}
private void AutoAdjAngle(int id)
{
if (id == 0)
{
AppendLog(LocUtil.FindResource("ubt.msgAutoAdjustNoBroadcast"));
return;
}
if (!robot.isConnected)
{
AppendLog(LocUtil.FindResource("ubt.msgAutoAdjustMustConnect"));
return;
}
if ((txtAutoAdjAngle.Text == null) || (txtAutoAdjAngle.Text.Trim() == ""))
{
AppendLog(LocUtil.FindResource("ubt.msgAutoAdjustRequireAngle"));
return;
}
try
{
int iFixAngle = (byte)UTIL.GetInputInteger(txtAutoAdjAngle.Text);
if (iFixAngle > CONST.UBT.MAX_ANGLE)
{
AppendLog(String.Format(LocUtil.FindResource("ubt.msgAutoInvalidAngle"), iFixAngle, CONST.UBT.MAX_ANGLE));
return;
}
// Get Crrent Adj Angle
byte[] buffer;
UInt16 adj;
if (!UBTGetAdjAngle(id, out adj, out buffer))
{
AppendLog(LocUtil.FindResource("ubt.msgGetAdjustFail"));
return;
}
int adjValue = 0;
if ((adj >= 0x0000) && (adj <= 0x0130))
{
adjValue = adj;
}
else if ((adj >= 0xFED0) && (adj <= 0xFFFF))
{
adjValue = (adj - 65536);
}
else
{
AppendLog(string.Format(LocUtil.FindResource("ubt.msgCurrentAdjustInvalid"), adj));
return;
}
byte currAngle;
if (!UBTGetAngle(id, out currAngle, out buffer))
{
AppendLog(LocUtil.FindResource("ubt.msgGetAngleFail"));
return;
}
int delta = cboAutoAdjDelta.SelectedIndex;
int actualValue = currAngle * 3 + adjValue;
int actualAngle = actualValue / 3;
int actualDelta = actualValue % 3;
int newValue = iFixAngle * 3 + delta;
int newAdjValue = actualValue - newValue;
if (!UBTSetAdjAngle(id, newAdjValue))
{
AppendLog(LocUtil.FindResource("ubt.msgSetAdjustFail"));
return;
}
AppendLog(string.Format(LocUtil.FindResource("ubt.msgAutoAdjustComplete"), id, actualAngle, actualDelta, iFixAngle, delta));
System.Threading.Thread.Sleep(100);
GetAdjAngle(id);
}
catch (Exception ex)
{
AppendLog("\nERR: " + ex.Message);
}
}
private void OnClick()
{
string property = "";
int c = 1;
//NAME
if (transform.GetChild(3).gameObject.transform.childCount == 3) //To compensate for the new object that appears when you give input
{
c = 2;
}
property = transform.GetChild(3).gameObject.transform.GetChild(c).GetComponent <Text>().text;
//property = property.Replace(" ", "");
if (property.Equals(""))
{
InvalidInput();
return;
}
scr_Map.NodeData node = new scr_Map.NodeData();
float size = 0;
UTIL.SplitNameStringAndGetFontSize(property, out node.NAME, out size);
//RACE
if (transform.GetChild(5).gameObject.transform.childCount == 3)
{
c = 2;
}
else
{
c = 1;
}
node.RACE = transform.GetChild(5).gameObject.transform.GetChild(c).GetComponent <Text>().text;
//BIRTHDATE
if (transform.GetChild(7).gameObject.transform.childCount == 3)
{
c = 2;
}
else
{
c = 1;
}
node.BIRTHDATE = transform.GetChild(7).gameObject.transform.GetChild(c).GetComponent <Text>().text;
//DEATHDATE
if (transform.GetChild(9).gameObject.transform.childCount == 3)
{
c = 2;
}
else
{
c = 1;
}
node.DEATHDATE = transform.GetChild(9).gameObject.transform.GetChild(c).GetComponent <Text>().text;
//BEMERKUNG
if (transform.GetChild(11).gameObject.transform.childCount == 3)
{
c = 2;
}
else
{
c = 1;
}
node.BEMERKUNG = transform.GetChild(11).gameObject.transform.GetChild(c).GetComponent <Text>().text;
//TAGS
if (transform.GetChild(12).gameObject.transform.childCount == 3)
{
c = 2;
}
else
{
c = 1;
}
node.TAGS = UTIL.ParseCommaString(transform.GetChild(12).gameObject.transform.GetChild(c).GetComponent <Text>().text);
node.POSITION_X = MetaVariables.mousePosition.x;
node.POSITION_Y = MetaVariables.mousePosition.y;
GameObject nodeObject = GameObject.Instantiate(
Resources.Load <GameObject>("Prefabs/NodePrefab"),
new Vector3(node.POSITION_X, node.POSITION_Y, 0),
Quaternion.Euler(0, 0, 0));
nodeObject.GetComponent <Node>().GiveData(node);
nodeObject.transform.GetChild(0).GetComponent <TextMesh>().characterSize = size;
scr_Map.AddNode(nodeObject.GetComponent <Node>(), false);
Destroy(gameObject);
}
// Read data fragment for load increment.
// newLoad = true - beginning of new load,
// = false - continuation of load.
// returns true if load data has been read
private bool readDataFile(FeScanner es, bool newLoad)
{
if (newLoad)
{
scaleLoad = 0;
nodForces = new List <Dof>();
itnf = nodForces.GetEnumerator();
surForces = new List <ElemFaceLoad>();
itsf = surForces.GetEnumerator();
if (fem.thermalLoading)
{
for (int i = 0; i < dtemp.Length; i++)
{
dtemp[i] = 0.0;
}
}
for (int i = 0; i < dDispl.Length; i++)
{
dDispl[i] = 0.0;
}
}
if (!es.hasNext())
{
return(false); // No load data
}
vars name = vars.NONE;
String s;
while (es.hasNext())
{
String varName = es.next();
String varNameLower = varName.ToLower();
if (varName.Equals("#"))
{
es.nextLine();
continue;
}
try {
name = (vars)System.Enum.Parse(typeof(vars), varNameLower); //vars.valueOf(varNameLower);
} catch (Exception E)
{
UTIL.errorMsg("Variable name is not found: " + varName);
}
switch (name)
{
case vars.loadstep:
loadStepName = es.next();
//Console.WriteLine("FELOAD " + loadStepName.ToString());
break;
case vars.scaleload:
scaleLoad = es.readDouble();
//Console.WriteLine("FELOAD " + scaleLoad.ToString());
break;
case vars.residtolerance:
residTolerance = es.readDouble();
//Console.WriteLine("FELOAD " + residTolerance.ToString());
break;
case vars.maxiternumber:
maxIterNumber = es.readInt();
//Console.WriteLine("FELOAD " + maxIterNumber.ToString());
break;
case vars.nodforce:
readNodalForces(es);
break;
case vars.surforce:
readSurForces(es);
break;
case vars.boxsurforce:
createBoxSurForces(es);
break;
case vars.nodtemp:
dtemp = new double[fem.nNod];
for (int i = 0; i < fem.nNod; i++)
{
dtemp[i] = es.readDouble();
}
break;
case vars.includefile:
s = es.next().ToLower();
FeScanner R = new FeScanner(s);
readDataFile(R, false);
break;
case vars.anglesurforce:
createAngleSurForce(es);
break;
case vars.surforcelist:
createSurForceList(es);
break;
case vars.elemanglesurforce:
createElemAngleSurForce(es);
break;
case vars.end:
return(true);
}
}
return(true);
}
// Assemble right-hand side of the global equation system
public void assembleRHS()
{
if (scaleLoad != 0.0)
{
for (int i = 0; i < fem.nEq; i++)
{
dpLoad[i] *= scaleLoad;
dhLoad[i] *= scaleLoad;
RHS [i] = dpLoad[i] + dhLoad[i];
}
return;
}
for (int i = 0; i < fem.nEq; i++)
{
dpLoad[i] = 0.0;
dhLoad[i] = 0.0;
}
// Nodal forces specified directly
itnf = nodForces.GetEnumerator();
Dof d;
while (itnf.MoveNext())
{
d = (Dof)itnf.Current;
dpLoad[d.dofNum - 1] = d.value;
}
// Surface load at element faces
itsf = surForces.GetEnumerator();
IEnumerator <ElemFaceLoad> itsf2 = surForces.GetEnumerator();
//IEnumerable<ElemFaceLoad> itsf = surForces.GetEnumerator();
ElemFaceLoad efl;
Element elm;
while (itsf.MoveNext())
{
efl = (ElemFaceLoad)itsf.Current;
elm = fem.elems[efl.iel];
elm.setElemXy();
if (elm.equivFaceLoad(efl) == -1)
{
UTIL.errorMsg("surForce does not match any face of element: " + efl.iel);
}
elm.assembleElemVector(Element.evec, dpLoad);
}
// Temperature field
if (fem.thermalLoading)
{
for (int iel = 0; iel < fem.nEl; iel++)
{
elm = fem.elems[iel];
elm.setElemXyT();
elm.thermalVector();
elm.assembleElemVector(Element.evec, dhLoad);
}
}
// Right-hand side = actual load + fictitious load
for (int i = 0; i < fem.nEq; i++)
{
RHS[i] = dpLoad[i] + dhLoad[i];
}
// Displacement boundary conditions for right-hand side
IEnumerator <Dof> itdbc = fem.defDs.GetEnumerator();
while (itnf.MoveNext())
{
d = (Dof)itdbc.Current;
RHS[d.dofNum - 1] = FE.bigValue * d.value;
}
}
private void Start()
{
player = UTIL.GetPlayer();
}
// Derivatives of shape functions
// with respect to global coordinates xy.
// xi, et, ze - local coordinates;
// ind - element connectivities;
// xy - nodal coordinates;
// dnxy - derivatives of shape functions (out);
// returns determinant of the Jacobian matrrix
public static double deriv(double xi, double et, double ze, int[] ind, double[][] xy, double[][] dnxy)
{
// Derivatives with respect to local coordinates d
double[][] d = new double[20][];
for (int i = 0; i < 20; i++)
{
d[i] = new double[3];
}
double s0 = 1 + xi;
double t0 = 1 + et;
double d0 = 1 + ze;
double s1 = 1 - xi;
double t1 = 1 - et;
double d1 = 1 - ze;
double s2 = 1 - xi * xi;
double t2 = 1 - et * et;
double d2 = 1 - ze * ze;
// Midside nodes
if (ind[1] > 0)
{
d[1][0] = -0.5 * xi * t1 * d1;
d[1][1] = -0.25 * s2 * d1;
d[1][2] = -0.25 * s2 * t1;
}
else
{
d[0][1] = d[1][1] = d[2][1] = 0;
}
if (ind[5] > 0)
{
d[5][0] = -0.5 * xi * t0 * d1;
d[5][1] = 0.25 * s2 * d1;
d[5][2] = -0.25 * s2 * t0;
}
else
{
d[5][0] = d[5][1] = d[5][2] = 0;
}
if (ind[17] > 0)
{
d[17][0] = -0.5 * xi * t0 * d0;
d[17][1] = 0.25 * s2 * d0;
d[17][2] = 0.25 * s2 * t0;
}
else
{
d[17][0] = d[17][1] = d[17][2] = 0;
}
if (ind[13] > 0)
{
d[13][0] = -0.5 * xi * t1 * d0;
d[13][1] = -0.25 * s2 * d0;
d[13][2] = 0.25 * s2 * t1;
}
else
{
d[13][0] = d[13][1] = d[13][2] = 0;
}
if (ind[7] > 0)
{
d[7][0] = -0.25 * t2 * d1;
d[7][1] = -0.5 * et * s1 * d1;
d[7][2] = -0.25 * t2 * s1;
}
else
{
d[7][0] = d[7][1] = d[7][2] = 0;
}
if (ind[3] > 0)
{
d[3][0] = 0.25 * t2 * d1;
d[3][1] = -0.5 * et * s0 * d1;
d[3][2] = -0.25 * t2 * s0;
}
else
{
d[3][0] = d[3][1] = d[3][2] = 0;
}
if (ind[15] > 0)
{
d[15][0] = 0.25 * t2 * d0;
d[15][1] = -0.5 * et * s0 * d0;
d[15][2] = 0.25 * t2 * s0;
}
else
{
d[15][0] = d[15][1] = d[15][2] = 0;
}
if (ind[19] > 0)
{
d[19][0] = -0.25 * t2 * d0;
d[19][1] = -0.5 * et * s1 * d0;
d[19][2] = 0.25 * t2 * s1;
}
else
{
d[19][0] = d[19][1] = d[19][2] = 0;
}
if (ind[8] > 0)
{
d[8][0] = -0.25 * d2 * t1;
d[8][1] = -0.25 * d2 * s1;
d[8][2] = -0.5 * ze * s1 * t1;
}
else
{
d[8][0] = d[8][1] = d[8][2] = 0;
}
if (ind[9] > 0)
{
d[9][0] = 0.25 * d2 * t1;
d[9][1] = -0.25 * d2 * s0;
d[9][2] = -0.5 * ze * s0 * t1;
}
else
{
d[9][0] = d[9][1] = d[9][2] = 0;
}
if (ind[10] > 0)
{
d[10][0] = 0.25 * d2 * t0;
d[10][1] = 0.25 * d2 * s0;
d[10][2] = -0.5 * ze * s0 * t0;
}
else
{
d[10][0] = d[10][1] = d[10][2] = 0;
}
if (ind[11] > 0)
{
d[11][0] = -0.25 * d2 * t0;
d[11][1] = 0.25 * d2 * s1;
d[11][2] = -0.5 * ze * s1 * t0;
}
else
{
d[11][0] = d[11][1] = d[11][2] = 0;
}
// Vertex nodes
d[0] [0] = -0.125 * t1 * d1 - 0.5 * (d[1] [0] + d[7] [0] + d[8] [0]);
d[0] [1] = -0.125 * s1 * d1 - 0.5 * (d[1] [1] + d[7] [1] + d[8] [1]);
d[0] [2] = -0.125 * s1 * t1 - 0.5 * (d[1] [2] + d[7] [2] + d[8] [2]);
d[2] [0] = 0.125 * t1 * d1 - 0.5 * (d[1] [0] + d[3] [0] + d[9] [0]);
d[2] [1] = -0.125 * s0 * d1 - 0.5 * (d[1] [1] + d[3] [1] + d[9] [1]);
d[2] [2] = -0.125 * s0 * t1 - 0.5 * (d[1] [2] + d[3] [2] + d[9] [2]);
d[4] [0] = 0.125 * t0 * d1 - 0.5 * (d[3] [0] + d[5] [0] + d[10][0]);
d[4] [1] = 0.125 * s0 * d1 - 0.5 * (d[3] [1] + d[5] [1] + d[10][1]);
d[4] [2] = -0.125 * s0 * t0 - 0.5 * (d[3] [2] + d[5] [2] + d[10][2]);
d[6] [0] = -0.125 * t0 * d1 - 0.5 * (d[5] [0] + d[7] [0] + d[11][0]);
d[6] [1] = 0.125 * s1 * d1 - 0.5 * (d[5] [1] + d[7] [1] + d[11][1]);
d[6] [2] = -0.125 * s1 * t0 - 0.5 * (d[5] [2] + d[7] [2] + d[11][2]);
d[12][0] = -0.125 * t1 * d0 - 0.5 * (d[8] [0] + d[13][0] + d[19][0]);
d[12][1] = -0.125 * s1 * d0 - 0.5 * (d[8] [1] + d[13][1] + d[19][1]);
d[12][2] = 0.125 * s1 * t1 - 0.5 * (d[8] [2] + d[13][2] + d[19][2]);
d[14][0] = 0.125 * t1 * d0 - 0.5 * (d[9] [0] + d[13][0] + d[15][0]);
d[14][1] = -0.125 * s0 * d0 - 0.5 * (d[9] [1] + d[13][1] + d[15][1]);
d[14][2] = 0.125 * s0 * t1 - 0.5 * (d[9] [2] + d[13][2] + d[15][2]);
d[16][0] = 0.125 * t0 * d0 - 0.5 * (d[10][0] + d[15][0] + d[17][0]);
d[16][1] = 0.125 * s0 * d0 - 0.5 * (d[10][1] + d[15][1] + d[17][1]);
d[16][2] = 0.125 * s0 * t0 - 0.5 * (d[10][2] + d[15][2] + d[17][2]);
d[18][0] = -0.125 * t0 * d0 - 0.5 * (d[11][0] + d[17][0] + d[19][0]);
d[18][1] = 0.125 * s1 * d0 - 0.5 * (d[11][1] + d[17][1] + d[19][1]);
d[18][2] = 0.125 * s1 * t0 - 0.5 * (d[11][2] + d[17][2] + d[19][2]);
// Modification of derivatives due to degeneration
if (degeneration(ind) == 1)
{
double[] dn1 = new double[3];
double[] dn2 = new double[3];
dn1[0] = -0.125 * xi * t2 * d1;
dn1[1] = -0.125 * et * s2 * d1;
dn1[2] = -0.0625 * s2 * t2;
dn2[0] = -0.125 * xi * t2 * d0;
dn2[1] = -0.125 * et * s2 * d0;
dn2[2] = -dn1[2];
for (int i = 0; i < 3; i++)
{
d[2] [i] += dn1[i];
d[3] [i] -= 2 * dn1[i];
d[4] [i] += dn1[i];
d[14][i] += dn2[i];
d[15][i] -= 2 * dn2[i];
d[16][i] += dn2[i];
}
}
// Jacobian matrix ja
double[][] ja = new double[3][] { new double[3], new double[3], new double[3] };
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
ja[j][i] = 0.0;
for (int k = 0; k < 20; k++)
{
ja[j][i] += d[k][i] * xy[k][j];
}
}
}
// Determinant of Jacobian matrix det
double det =
ja[0][0] * (ja[1][1] * ja[2][2] - ja[2][1] * ja[1][2])
- ja[1][0] * (ja[0][1] * ja[2][2] - ja[0][2] * ja[2][1])
+ ja[2][0] * (ja[0][1] * ja[1][2] - ja[0][2] * ja[1][1]);
if (det <= 0)
{
UTIL.errorMsg("Negative/zero Jacobian determinant for 20Nr element " + (float)det);
}
// Jacobian inverse ja1
double[][] ja1 = new double[3][] { new double[3], new double[3], new double[3] };
double v = 1.0 / det;
ja1[0][0] = (ja[1][1] * ja[2][2] - ja[2][1] * ja[1][2]) * v;
ja1[1][0] = (ja[0][2] * ja[2][1] - ja[0][1] * ja[2][2]) * v;
ja1[2][0] = (ja[0][1] * ja[1][2] - ja[0][2] * ja[1][1]) * v;
ja1[0][1] = (ja[1][2] * ja[2][0] - ja[1][0] * ja[2][2]) * v;
ja1[1][1] = (ja[0][0] * ja[2][2] - ja[2][0] * ja[0][2]) * v;
ja1[2][1] = (ja[0][2] * ja[1][0] - ja[0][0] * ja[1][2]) * v;
ja1[0][2] = (ja[2][1] * ja[1][0] - ja[2][0] * ja[1][1]) * v;
ja1[1][2] = (ja[0][1] * ja[2][0] - ja[0][0] * ja[2][1]) * v;
ja1[2][2] = (ja[0][0] * ja[1][1] - ja[1][0] * ja[0][1]) * v;
for (int k = 0; k < 20; k++)
{
for (int i = 0; i < 3; i++)
{
dnxy[k][i] = 0.0;
for (int j = 0; j < 3; j++)
{
dnxy[k][i] += ja1[i][j] * d[k][j];
}
}
}
return(det);
}
// Set sparse row structure for storage of nonzero
// coefficients of the global stiffness matrix.
private void setSparseRowStructure()
{
prow = new int[neq + 1];
int lrow = fem.nDf * ((fem.nDf == 2) ? FE.maxRow2D : FE.maxRow3D);
coln = new int[neq * lrow];
for (int i = 0; i <= fem.nNod; i++)
{
prow[i] = i * lrow * fem.nDf;
}
// Create nodal sparse-row matrix structure
for (int i = 0; i < prow[fem.nNod]; i++)
{
coln[i] = -1;
}
// Diagonal entry - first in row
for (int i = 0; i < fem.nNod; i++)
{
coln[prow[i]] = i;
}
for (int iel = 0; iel < fem.nEl; iel++)
{
foreach (int anInd in fem.elems[iel].ind)
{
if (anInd == 0)
{
continue;
}
int ii = anInd - 1; // Hyperrow
foreach (int anInd1 in fem.elems[iel].ind)
{
if (anInd1 == 0)
{
continue;
}
int jj = anInd1 - 1; // Hypercolumn
int k;
for (k = prow[ii]; k < prow[ii + 1]; k++)
{
// If column already exists
if (coln[k] == jj)
{
break;
}
if (coln[k] == -1)
{
coln[k] = jj;
break;
}
}
if (k == prow[ii + 1])
{
UTIL.errorMsg("PCG sparse-row structure: not enough space for node " + ii);
}
}
}
}
// Compress
int p = 0;
for (int i = 0; i < fem.nNod; i++)
{
int k = prow[i];
prow[i] = p;
for (int j = k; j < prow[i + 1]; j++)
{
if (coln[j] == -1)
{
break;
}
coln[p++] = coln[j];
}
}
prow[fem.nNod] = p;
// Transform to degrees of freedom
int pdof = p * fem.nDf * fem.nDf;
for (int i = fem.nNod - 1; i >= 0; i--)
{
int deln = (prow[i + 1] - prow[i]);
p -= deln;
for (int k = fem.nDf; k > 0; k--)
{
prow[i * fem.nDf + k] = pdof;
pdof -= deln * fem.nDf;
for (int j = prow[i + 1] - prow[i] - 1; j >= 0; j--)
{
for (int m = fem.nDf - 1; m >= 0; m--)
{
coln[pdof + j * fem.nDf + m] =
coln[p + j] * fem.nDf + m;
}
}
}
}
lengthOfGSM = (int)(prow[neq] * 1.5);
}
// Set nodal equivalent of distributed face load to evec.
// surLd - object describing element face load;
// returns loaded element face
// or -1 (loaded nodes does not match element face)
public override int equivFaceLoad(ElemFaceLoad surLd)
{
// Shape functons
double[] an = new double[8];
// Derivatives of shape functions
double[][] xin = new double[8][] { new double[2], new double[2], new double[2], new double[2],
new double[2], new double[2], new double[2], new double[2] };
// Tangent vectors along xi and eta
double[][] e = new double[2][] { new double[3], new double[3] };
// Normal vector
double[] g = new double[3];
double[] ps = new double[3];
for (int i = 0; i < 60; i++)
{
evec[i] = 0.0;
}
int loadedFace = surLd.rearrange(faceInd, ind);
if (loadedFace == -1)
{
return(-1);
}
for (int ip = 0; ip < gf.nIntPoints; ip++)
{
ShapeQuad3D.shapeDerivFace(gf.xii[ip], gf.eti[ip], ind, an, xin);
double p = 0.0;
for (int i = 0; i < 8; i++)
{
p += an[i] * surLd.forceAtNodes[i];
}
// Tangent vectors
for (int i = 0; i < 2; i++)
{
for (int j = 0; j < 3; j++)
{
double s = 0;
for (int k = 0; k < 8; k++)
{
s += xin[k][i] * xy[faceInd[loadedFace][k]][j];
}
e[i][j] = s;
}
}
// Normal vector g
g[0] = (e[0][1] * e[1][2] - e[1][1] * e[0][2]);
g[1] = (e[0][2] * e[1][0] - e[1][2] * e[0][0]);
g[2] = (e[0][0] * e[1][1] - e[1][0] * e[0][1]);
// Element of surface ds
double ds = Math.Sqrt(g[0] * g[0] + g[1] * g[1] + g[2] * g[2]);
if (ds <= 0)
{
UTIL.errorMsg("Negative/zero element face");
}
// Surface load components ps:
// direction=0 - normal, x=1, y=2, z=3
if (surLd.direction == 0)
{
for (int i = 0; i < 3; i++)
{
ps[i] = p * g[i] / ds;
}
}
else
{
for (int i = 0; i < 3; i++)
{
ps[i] = 0;
}
ps[surLd.direction - 1] = p;
}
for (int i = 0; i < 8; i++)
{
int k = faceInd[loadedFace][i];
for (int j = 0; j < 3; j++)
{
evec[3 * k + j] += an[i] * ps[j] * ds * gf.wi[ip];
}
}
}
return(loadedFace);
}
public void OpenCellEditor()
{
UTIL.OpenCellEditor("");
}
private void readData()
{
String varName, varname;
while (CSMGEN.RD.hasNext())
{
varName = CSMGEN.RD.next();
varname = varName.ToLower();
if (varName.Equals("#"))
{
CSMGEN.RD.nextLine(); continue;
}
try
{
//name = vars.valueOf(varname);
name = (vars)System.Enum.Parse(typeof(vars), varname);
}
catch (Exception e)
{
UTIL.errorMsg("Variable name is not found: " + varName);
}
switch (name)
{
case vars.nx:
nx = CSMGEN.RD.readInt();
break;
case vars.ny:
ny = CSMGEN.RD.readInt();
break;
case vars.xs:
xs = new double[nx + 1];
for (int i = 0; i <= nx; i++)
{
xs[i] = CSMGEN.RD.readDouble();
}
break;
case vars.ys:
ys = new double[ny + 1];
for (int i = 0; i <= ny; i++)
{
ys[i] = CSMGEN.RD.readDouble();
}
break;
case vars.mat:
mat = CSMGEN.RD.next();
break;
case vars.thick:
t = CSMGEN.RD.readDouble();
break;
case vars.end:
return;
}
}
}
private void aiDrive(List <Robot> robots, List <Ball> balls)
{
if (this.previousPosition.Equals(location))
{
stuckCount++;
}
else
{
stuckCount = 0;
}
bool moved = false;
rotation = (float)UTIL.normalizeDirection(rotation);
float turnAxis, strafeAxis, powerAxis;
bool fire = false;
turnAxis = strafeAxis = powerAxis = 0f;
AICommand command = aiHandler.get();
if (!command.Equals(previous))
{
cycles = 0;
}
cycles++;
if (cycles > command.getTimeout())
{
aiHandler.move();
moved = true;
command = aiHandler.get();
cycles = 0;
}
Vector2 target = Vector2.Zero;
if (command.getType() == AICommand.driveCommand)
{
target = (Vector2)command.getValue();
powerAxis = (float)aiDrivePID.calcPID(UTIL.distance(location, target));
double goal = UTIL.normalizeDirection(UTIL.getDirectionTward(location, target));
if (Math.Abs(goal + 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal += 2 * Math.PI;
}
if (Math.Abs(goal - 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal -= 2 * Math.PI;
}
aiTurnPID.setDesiredValue(goal);
turnAxis = (float)aiTurnPID.calcPID(rotation);
if (UTIL.distance(target, location) < 35 && !moved)
{
moved = true;
aiHandler.move();
}
}
else if (command.getType() == AICommand.fireCommand)
{
double goal = 0.0;
if (color.Equals(Color.Blue))
{
goal = Math.PI;
}
if (Math.Abs(goal + 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal += 2 * Math.PI;
}
if (Math.Abs(goal - 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal -= 2 * Math.PI;
}
aiTurnPID.setDesiredValue(goal);
turnAxis = (float)aiTurnPID.calcPID(rotation);
powerAxis = (aiTurnPID.isDone()) ? -0.3f : 0.25f;
fire = aiTurnPID.isDone();
if (activeBall == null && !moved)
{
aiHandler.move();
moved = true;
}
}
else if (command.getType() == AICommand.passCommand)
{
double goal = 0.0;
double minDistance = double.MaxValue;
foreach (Robot r in robots)
{
if (!r.Equals(this) && r.getColor().Equals(color))
{
if (UTIL.distance(location, r.getLocation()) < minDistance)
{
minDistance = UTIL.distance(location, r.getLocation());
goal = UTIL.getDirectionTward(location, r.getLocation());
}
}
}
if (Math.Abs(goal + 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal += 2 * Math.PI;
}
if (Math.Abs(goal - 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal -= 2 * Math.PI;
}
aiTurnPID.setDesiredValue(goal);
turnAxis = (float)aiTurnPID.calcPID(rotation);
fire = aiTurnPID.isDone();
if (!moved && (activeBall == null || activeBall.getAssistBonus() >= 10))
{
aiHandler.move();
}
}
else if (command.getType() == AICommand.positionCommand)
{
Vector2 offSet = (Vector2)command.getValue();
Vector2 ballCoordinate = Vector2.Zero;
double minDistance = double.MaxValue;
foreach (Ball b in balls)
{
if (b.getColor().Equals(color))
{
if (UTIL.distance(location, b.getLocation()) < minDistance && ((offSet.Equals(Vector2.Zero)) ? b.getIsFree() : !b.getIsFree()))
{
target = b.getLocation() + offSet;
if (primaryZone == RedPrimary)
{
if (target.X > redZone * widthScale - zoneBleed * widthScale)
{
ballCoordinate = b.getLocation();
minDistance = UTIL.distance(ballCoordinate, location);
}
}
else if (primaryZone == WhitePrimary)
{
if (target.X > blueZone * widthScale - zoneBleed * widthScale && target.X < redZone * widthScale + zoneBleed * widthScale)
{
ballCoordinate = b.getLocation();
minDistance = UTIL.distance(ballCoordinate, location);
}
}
else if (primaryZone == BluePrimary)
{
if (target.X < blueZone * widthScale + zoneBleed * widthScale)
{
ballCoordinate = b.getLocation();
minDistance = UTIL.distance(ballCoordinate, location);
}
}
}
}
}
target = ballCoordinate + offSet;
powerAxis = (float)aiDrivePID.calcPID(UTIL.distance(location, target));
double goal = UTIL.getDirectionTward(location, target);
if (Math.Abs(goal + 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal += 2 * Math.PI;
}
if (Math.Abs(goal - 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal -= 2 * Math.PI;
}
aiTurnPID.setDesiredValue(goal);
turnAxis = (float)aiTurnPID.calcPID(rotation);
if (!moved && activeBall != null || ballCoordinate.Equals(Vector2.Zero))
{
moved = true;
aiHandler.move();
}
}
else if (command.getType() == AICommand.defenseCommand)
{
Vector2 offSet = Vector2.Zero;
Vector2 ballCoordinate = Vector2.Zero;
double minDistance = double.MaxValue;
foreach (Ball b in balls)
{
if (!b.getColor().Equals(color))
{
if (UTIL.distance(location, b.getLocation()) < minDistance)
{
ballCoordinate = b.getLocation();
minDistance = UTIL.distance(ballCoordinate, location);
}
}
}
target = ballCoordinate + offSet;
powerAxis = (float)aiDrivePID.calcPID(UTIL.distance(location, target));
double goal = UTIL.getDirectionTward(location, target);
if (Math.Abs(goal + 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal += 2 * Math.PI;
}
if (Math.Abs(goal - 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal -= 2 * Math.PI;
}
aiTurnPID.setDesiredValue(goal);
turnAxis = (float)aiTurnPID.calcPID(rotation);
if (activeBall != null && !moved)
{
moved = true;
aiHandler.move();
}
}
if (!target.Equals(Vector2.Zero) && !moved)
{
if (primaryZone == RedPrimary)
{
if (target.X < redZone * widthScale - zoneBleed * widthScale)
{
aiHandler.move();
moved = true;
}
}
else if (primaryZone == WhitePrimary)
{
if (target.X < blueZone * widthScale - zoneBleed * widthScale || target.X > redZone * widthScale + zoneBleed * widthScale)
{
aiHandler.move();
moved = true;
}
}
else if (primaryZone == BluePrimary)
{
if (target.X > blueZone * widthScale + zoneBleed * widthScale)
{
aiHandler.move();
moved = true;
}
}
}
if (!moved || fire)
{
drive(robots, balls, turnAxis, powerAxis, strafeAxis);
}
else
{
drive(robots, balls, 0, 0, 0);
}
if (stuckCount >= 200)
{
turnAxis = -0.5f;
powerAxis = -1.0f;
strafeAxis = 0.0f;
drive(robots, balls, turnAxis, powerAxis, strafeAxis);
}
if (activeBall != null && fire)
{
activeBall.launch(new Vector3(launchPower * (float)Math.Cos(rotation) + velocity.X, launchPower * (float)Math.Sin(rotation) + velocity.Y, 2f));
activeBall = null;
launch.Play();
}
previous = command;
}
void ShakeCamera(float duration)
{
UTIL.GetPlayer().GetComponent <CameraShake>().Shake(duration);
}
// Derivatives of shape functions
// with respect to global coordinates x and y.
// xi, et - local coordinates;
// ind[4] - element connectivities;
// xy[4][2] - nodal coordinates;
// dnxy[4][2] - derivatives of shape functions (out);
// returns determinant of the Jacobian matrrix
public static double deriv(double xi, double et, int[] ind, double[][] xy, double[][] dnxy)
{
// Derivatives in local coords dN/dXi, dN/dEta
// Midside nodes
double[][] dnxe = new double[4][] { new double[2], new double[2], new double[2], new double[2] };
// Corner nodes
dnxe[0][0] = -0.25 * (1.0 - et);
dnxe[0][1] = -0.25 * (1.0 - xi);
dnxe[1][0] = 0.25 * (1.0 - et);
dnxe[1][1] = -0.25 * (1.0 + xi);
dnxe[2][0] = 0.25 * (1.0 + et);
dnxe[2][1] = 0.25 * (1.0 + xi);
dnxe[3][0] = -0.25 * (1.0 + et);
dnxe[3][1] = 0.25 * (1.0 - xi);
// Modification of derivatives due to degeneration
int deg = degeneration(ind);
if (deg > 0)
{
double sum0 = dnxe[deg - 1][0] + dnxe[deg][0];
double sum1 = dnxe[deg - 1][1] + dnxe[deg][1];
dnxe[deg - 1][0] = dnxe[deg][0] = sum0;
dnxe[deg - 1][1] = dnxe[deg][1] = sum1;
}
// Jacobian matrix
double[][] aj = new double[2][] { new double[2], new double[2] };
for (int j = 0; j < 2; j++)
{
for (int i = 0; i < 2; i++)
{
aj[i][j] = 0.0;
for (int k = 0; k < 4; k++)
{
aj[i][j] += dnxe[k][j] * xy[k][i];
}
}
}
double det = aj[0][0] * aj[1][1] - aj[0][1] * aj[1][0];
// Zero or negative determinant
if (det <= 0)
{
UTIL.errorMsg("Negative/zero Jacobian determinant for 8N element " + (float)det);
}
// Jacobian inverse
double aj00 = aj[1][1] / det;
aj[1][1] = aj[0][0] / det;
aj[0][0] = aj00;
aj[1][0] = -aj[1][0] / det;
aj[0][1] = -aj[0][1] / det;
// Derivatives in global coordinates dN/dx, dN/dy
for (int k = 0; k < 4; k++)
{
for (int i = 0; i < 2; i++)
{
dnxy[k][i] = aj[0][i] * dnxe[k][0] + aj[1][i] * dnxe[k][1];
}
}
return(det);
}
// Update is called once per frame
void Update() //----매 프레임마다 한번씩 실행----
{
bool inSight = false;
/*
* bool 타입 변수 inSight
* inSight = false: 시야에 안보임, inSight = true: 보임
*/
if (!selfRaycast)
{
foreach (Transform pos in raycastPos) //raycastPos라는 리스트 안에 있는 모든 Transform 타입 변수(pos)에 대해
{
RaycastHit2D cast = Physics2D.Raycast(pos.position, target.position - pos.position, UTIL.FastDist(target.position, pos.position, 0.05f), wallLayers);
/*
* RaycastHit2D 타입 변수 cast
* pos의 위치에서 Transform 타입 변수 target(플레이어)의 방향으로 target(플레이어) 까지의 거리만큼
* 레이저를 wallLayers(벽 레이어)에서 발사
*/
if ((cast.collider == null && UTIL.FastDist(target.position, pos.position, 0.05f) <= maxDist) || UTIL.FastDist(target.position, pos.position, 0.05f) <= showDist)
{
inSight = true;
}
/*
* 만약 레이저에 아무것도 닿지 안았고 발사 지점과 너무 멀지 않았다면:
* inSight를 참으로 설정한다 => 보이는 거임
*/
}
}
else
{
RaycastHit2D cast = Physics2D.Raycast(transform.position, target.position - transform.position, UTIL.FastDist(target.position, transform.position, 0.05f), wallLayers);
if ((cast.collider == null && UTIL.FastDist(target.position, transform.position, 0.05f) <= maxDist) || UTIL.FastDist(target.position, transform.position, 0.05f) <= showDist)
{
inSight = true;
}
}
//-------애니메이션(사라지기/보이기 효과)---------
if (useAnim)
{
if (inSight)
{
anim.SetBool("InSight", true);
}
else
{
anim.SetBool("InSight", false);
}
}
else
{
if (inSight)
{
sr.enabled = true;
}
else
{
sr.enabled = false;
}
}
//Debug.Log("InSight: " + inSight);
}
public void OpenMapShowcase()
{
UTIL.OpenScene("MapGenerator_TEST");
}
