const Matrix &
BeamEndContact3D::getTangentStiff(void)
// this function computes the tangent stiffness matrix for the element
{
mTangentStiffness.Zero();
if (inContact) {
for (int i = 0; i <= 2; i++) {
mTangentStiffness(i,9) = mNormal(i);
mTangentStiffness(i+6,9) = -mNormal(i);
mTangentStiffness(9,i) = mNormal(i);
mTangentStiffness(9,i+6) = -mNormal(i);
}
mTangentStiffness(10,10) = 1.0;
mTangentStiffness(11,11) = 1.0;
} else {
mTangentStiffness(9,9) = 1.0;
mTangentStiffness(10,10) = 1.0;
mTangentStiffness(11,11) = 1.0;
}
return mTangentStiffness;
}
Plane<T>& Plane<T>::fromCoefficients(T a, T b, T c, T d){
mNormal(a,b,c);
T l = mNormal.mag();
mNormal(a/l,b/l,c/l);
mD = d/l;
return *this;
}
const Vector &
BeamEndContact3Dp::getResistingForce()
// this function computes the resisting force vector for the element
{
mInternalForces.Zero();
if (inContact) {
for (int i = 0; i < 3; i++) {
mInternalForces(i) = -mLambda*mNormal(i);
mInternalForces(i+6) = mLambda*mNormal(i);
}
}
return mInternalForces;
}
const Matrix &
BeamEndContact3Dp::getTangentStiff(void)
// this function computes the tangent stiffness matrix for the element
{
mTangentStiffness.Zero();
double tangentTerm;
if (inContact) {
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
tangentTerm = mPenalty*mNormal(i)*mNormal(j);
mTangentStiffness(i,j) = tangentTerm;
mTangentStiffness(i,6+j) = -tangentTerm;
mTangentStiffness(6+i,j) = -tangentTerm;
mTangentStiffness(6+i,6+j) = tangentTerm;
}
}
}
return mTangentStiffness;
}
void cPlane::render() const
{
glPushMatrix();
Eigen::Vector3f mPos = -mNormal * mD;
Eigen::Vector3f eX, eY;
if (mD > 1e-6f)
{
Eigen::Vector3f v(10, 10, 10);
int i = -1;
float m = 0;
if (fabs(mNormal.x()) > 1e-6f)
i = 0;
else if (fabs(mNormal.y()) > 1e-6f)
i = 1;
else if (fabs(mNormal.z()) > 1e-6f)
i = 2;
m = mNormal(i), v(i) = 0;
m = (mD + v.dot(mNormal)) / m;
v(i) = m;
eX = v;
}
else
{
eX = Eigen::Vector3f(0, 0, 1);
}
eX = (eX + mPos).normalized();
eY = mNormal.cross(eX).normalized();
float W = 50;
float H = 50;
glTranslatef((GLfloat)mPos.x(), (GLfloat)mPos.y(), (GLfloat)mPos.z());
setupMaterial();
//glutSolidSphere(0.01, 20, 20);
glBegin(GL_QUADS);
glVertex3fv(Eigen::Vector3f(-W * eX - H * eY).data());
glVertex3fv(Eigen::Vector3f(-W * eX + H * eY).data());
glVertex3fv(Eigen::Vector3f(W * eX + H * eY).data());
glVertex3fv(Eigen::Vector3f(W * eX - H * eY).data());
glEnd();
glPopMatrix();
}
void
BeamContact2Dp::ComputeB(void)
// this function computes the finite element equation vectors Bn and Bs
{
double Ka1n;
double Kb1n;
double Ka1g;
double Kb1g;
Vector a1(BC2D_NUM_DIM);
Vector b1(BC2D_NUM_DIM);
// initialize Bn and Bs
mBn.Zero();
mBs.Zero();
// get tangent vectors
a1 = Geta1();
b1 = Getb1();
// compute terms for vector Bn
Ka1n = (mEyeS*a1)^mNormal;
Kb1n = (mEyeS*b1)^mNormal;
mBn(0) = -mShape(0)*mNormal(0);
mBn(1) = -mShape(0)*mNormal(1);
mBn(2) = -mShape(1)*mLength*Ka1n;
mBn(3) = -mShape(2)*mNormal(0);
mBn(4) = -mShape(2)*mNormal(1);
mBn(5) = -mShape(3)*mLength*Kb1n;
mBn(6) = mNormal(0);
mBn(7) = mNormal(1);
// compute terms for vector Bs
Ka1g = (mEyeS*a1)^mg_xi;
Kb1g = (mEyeS*b1)^mg_xi;
mBs(0) = -mg_xi(0)*(mShape(0) + mRadius*mDshape(0));
mBs(1) = -mg_xi(1)*(mShape(0) + mRadius*mDshape(0));
mBs(2) = -Ka1g*mLength*(mShape(1) + mRadius*mDshape(1));
mBs(3) = -mg_xi(0)*(mShape(2) + mRadius*mDshape(2));
mBs(4) = -mg_xi(1)*(mShape(2) + mRadius*mDshape(2));
mBs(5) = -Kb1g*mLength*(mShape(3) + mRadius*mDshape(3));
mBs(6) = mg_xi(0);
mBs(7) = mg_xi(1);
return;
}
